void calibratePositionToKinect()
{
if ( ! this->checkKinect( "CalibratePositionToKinect" ) ) return;
this->pBrain->drive()->setStopWithin( 0 );
std::cerr << "Performing odometry calibration, please wait..." << std::endl;
this->pBrain->cbha()->grip();
this->pBrain->drive()->niceStop();
sleep( 4 );
// Make sure Robotino is not driving
while ( this->pBrain->odom()->currentAbsSpeed() > 0.01 || this->pBrain->odom()->currentAbsOmega() > 0.01 )
usleep( 100000 );
/// @todo Improvement: Use current coordinate system, so this is not lost if calibration is aborted (not currently applicable).
this->pBrain->odom()->set( 0.0, 0.0, 0.0 );
this->pBrain->cbha()->innerToCoordinate( 0.0, 0.4 );
this->pBrain->cbha()->outerToCoordinate( 0.0, 0.4 );
std::cerr << "Wait for arm to reach position..." << std::endl;
while ( this->pBrain->cbha()->armTotalPressureDiff() > 0.1 )
usleep( 100000 );
std::cerr
<< "Stand beside Robotino and make sure Kinect is reading your hand.\n"
<< "Then, using your wrist, slightly push down on the tip of Robotinos gripper.\n"
<< "Robotino will, after a slight pause, drive 1 meter forward"
<< std::endl;
VolumeCoordinate kinectCoordinate0 = this->pBrain->cbha()->getTouchCoordinate();
std::cerr << "First coordinate stored : " << kinectCoordinate0 << "\nGet out of my way!" << std::endl;
this->pBrain->cbha()->innerToCoordinate( 0.0, 0.2 );
this->pBrain->cbha()->outerToCoordinate( 0.0, 0.2 );
usleep ( 500000 );
this->pBrain->drive()->setDestination( Coordinate( 1.0, 0.0 ) );
this->pBrain->drive()->go();
this->pBrain->drive()->setPointAt( Coordinate( 1000.0, 0.0 ) );
sleep ( 2 );
this->pBrain->cbha()->innerToCoordinate( 0.0, 0.4 );
this->pBrain->cbha()->outerToCoordinate( 0.0, 0.4 );
// Wait until Robotino is in position
while ( this->pBrain->odom()->currentAbsSpeed() > 0.01 || this->pBrain->odom()->currentAbsOmega() > 0.01 )
usleep( 100000 );
this->pBrain->drive()->niceStop();
std::cerr << "Wait for arm to reach position..." << std::endl;
while ( this->pBrain->cbha()->armTotalPressureDiff() > 0.1 )
usleep( 100000 );
std::cerr
<< "Again, using your wrist tracked by Kinect, slightly push down on the tip of Robotinos gripper."
<< std::endl;
VolumeCoordinate kinectCoordinate1 = this->pBrain->cbha()->getTouchCoordinate();
std::cerr << "Second coordinate stored: " << kinectCoordinate1 << std::endl;
// Get current odom position
AngularCoordinate odomPos1 = this->pBrain->odom()->getPosition();
// Calculate actual heading and position:
// This is done using the now known travel direction of Robotino, and
// the approximate distance between the touched arm and Robotinos
// center.
Vector odomDeviation = Coordinate( 0.0, 1.0 ).getVector( odomPos1 );
Angle phi;
Coordinate kinectCoordinate0Adjusted = kinectCoordinate0;
float kinectAngleDiff = 99.0;
while ( true )
{
Vector kinectVector = kinectCoordinate0Adjusted.getVector( kinectCoordinate1 );
kinectAngleDiff -= fabs( kinectVector.phi() );
kinectAngleDiff = fabs( kinectAngleDiff );
phi = Angle( kinectVector.phi() + odomPos1.phi() );
Coordinate convertedOdomDeviation = Vector( odomDeviation.magnitude(), phi.phi() ).cartesian();
kinectCoordinate0Adjusted = Coordinate(
kinectCoordinate0.x() + convertedOdomDeviation.x(),
kinectCoordinate0.y() + convertedOdomDeviation.y() );
if ( kinectAngleDiff > 0.01 ) break;
kinectAngleDiff = fabs( kinectVector.phi() );
}
// Calculate the arm offset to apply to the second kinect position
Coordinate armVector = Vector( CONTROL_CALIBRATE_ARM_DISPLACEMENT, phi.phi() ).cartesian();
// Calculate and apply coordinates and vector
float x = kinectCoordinate1.x() - armVector.x();
float y = kinectCoordinate1.y() - armVector.y();
if ( this->pBrain->odom()->set( x, y, phi.phi() ) )
{
usleep( 200000 ); // Give set a moment to take effect
std::cerr << "Calibration completed, new position set: " << this->pBrain->odom()->getPosition() << std::endl;
}
this->pBrain->cbha()->armRelax();
this->pBrain->cbha()->release();
this->pBrain->drive()->setDestination( this->pBrain->odom()->getPosition() );
this->pBrain->drive()->stopPointing();
}
