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