motorVels SwerveDrive::steer(motorVels vels, double velMagnitude, double turnAngle) {
const double rotationRadius = HALF_ROVER_WIDTH_X/sin(turnAngle);
geometry_msgs::Vector3 rotationCentre;
rotationCentre.x = -HALF_ROVER_WIDTH_X;
rotationCentre.y = sqrt(pow(rotationRadius,2)-pow(HALF_ROVER_WIDTH_X,2));
//magnitude velocity over rotation radius
const double angularVelocity = velMagnitude / rotationRadius;
ROS_INFO("turnAngle %lf, rotationRadius %lf, rotationCenter %lf, %lf, %lf",turnAngle, rotationRadius, rotationCentre.x, rotationCentre.y, rotationCentre.z);
//calculate the radiuses of each wheel about the rotation center
//NOTE: if necisary this could be optimised
double closeBackR = fabs(rotationCentre.y - ROVER_CENTRE_2_WHEEL_Y);
double farBackR = fabs(rotationCentre.y + ROVER_CENTRE_2_WHEEL_Y);
double closeFrontR = getHypotenuse(closeBackR, FRONT_W_2_BACK_W_X);
double farFrontR = getHypotenuse(farBackR, FRONT_W_2_BACK_W_X);
//V = wr
double closeBackV = closeBackR * angularVelocity;
double farBackV = farBackR * angularVelocity;
double closeFrontV = closeFrontR * angularVelocity;
double farFrontV = farFrontR * angularVelocity;
//work out the front wheel angles
double closeFrontAng = DEG90-atan2(closeBackR,FRONT_W_2_BACK_W_X);
double farFrontAng = DEG90-atan2(farBackR,FRONT_W_2_BACK_W_X);
//if we are in reverse, we just want to go round the same circle in the opposite direction (I think...)
if(direction == BACK) {
//flip all the motorVs
closeFrontV *=-1.0;
farFrontV *=-1.0;
farBackV *=-1.0;
closeBackV *=-1.0;
}
motorVels output = vels;
//work out which side to favour
if(0 <= turnAngle && turnAngle <= M_PI) {
output.frontLeftMotorV = closeFrontV;
output.backLeftMotorV = closeBackV;
output.frontRightMotorV = farFrontV;
output.backRightMotorV = farBackV;
output.frontLeftAng = closeFrontAng;
output.frontRightAng = farFrontAng;
ROS_INFO("right");
} else {
output.frontRightMotorV = -closeFrontV;
output.backRightMotorV = -closeBackV;
output.frontLeftMotorV = -farFrontV;
output.backLeftMotorV = -farBackV;
output.frontLeftAng = -farFrontAng;
output.frontRightAng = -closeFrontAng;
ROS_INFO("left");
}
output.backLeftAng = 0;
output.backRightAng = 0;
return output;
}
// this takes an already calculated object distance (via blob height/width)
// to the camera focal point plus its center x,y on the screen
// and returns the distance,bearing, and elevation to the center of the body.
// no sanity checks done in this method
const estimate Pose::bodyEstimate(const int pixel_x, const int pixel_y,
const float obj_dist) {
// check if distance is ridiculous
if (obj_dist <= 0.0)
return NULL_ESTIMATE;
// get bearing in image plane
float object_bearing = DEG2RAD((IMAGE_WIDTH/2 - pixel_x)/MAX_BEARING);
// get elevation in image plane
float object_elevation = DEG2RAD((IMAGE_HEIGHT/2 - pixel_y)/MAX_ELEVATION);
// convert dist estimate to mm
float object_dist = obj_dist*10;
// declare (x,y,z) of object in the image frame
point3 <float> object_in_image_frame;
// get (x,y,z) of object in image plane
object_in_image_frame.x = -object_dist*sin(object_bearing);
object_in_image_frame.y =
object_dist*cos(object_bearing)*cos(object_elevation);
object_in_image_frame.z =
object_dist*cos(object_bearing)*sin(object_elevation);
// transform (x,y,z) to body center
point3 <float> object_in_body_frame = image_matrix.transform(object_in_image_frame);
//print("coords x: %g y: %g z: %g", coords.x, coords.y, coords.z);
// declare estimate
estimate est = {0,0,0,0,0};
// get distnace from (x,y,z) of object to body center (origin);
//est.dist = get3dDist(object_in_body_frame,ZERO_COORD);
// get distance from just the x/y plane
est.dist = getHypotenuse(object_in_body_frame.x,object_in_body_frame.y);
// get elevation from body center
est.elevation = RAD2DEG(asin(object_in_body_frame.z/est.dist));
// we'll estimate bearing based off the shorter of x and y
if (abs(object_in_body_frame.x) < abs(object_in_body_frame.y)) {
est.bearing = RAD2DEG(asin(object_in_body_frame.x/est.dist));
}
else {
est.bearing = RAD2DEG(acos(object_in_body_frame.y/est.dist));
if (object_in_body_frame.x < 0) est.bearing = -est.bearing;
}
// convert dist back into cms
est.dist = est.dist / 10.0;
//print("coords x: %g y: %g z: %g body:: dist: %g elevation: %g bearing: %g", coords.x, coords.y, coords.z, est.dist/10.0, est.elevation, est.bearing);
return est;
}
// returns 'estimate' (dist,bearing,elevation) to a pixel x,y in centimeters
// from body center
const estimate Pose::pixEstimate(const int pixel_x,const int pixel_y,
const float object_height) {
// verifies that pixel is on screen. if it isn't, null estimate
if (pixel_x > IMAGE_WIDTH || pixel_x < 0 ||
pixel_y > IMAGE_HEIGHT || pixel_y < 0) {
//print("pixel out of range, x: %d y: %d", pixel_x, pixel_y);
return NULL_ESTIMATE;
}
// declare x,y,z coord of pixel in relation to focal point
point3 <float> pixel_in_camera_frame;
// calculate camera frame x,y,z on a specific pixel
pixel_in_camera_frame.x = pixel_x - IMAGE_CENTER_X;
pixel_in_camera_frame.y = FOCAL_LENGTH_MM;
pixel_in_camera_frame.z = IMAGE_CENTER_Y - pixel_y;
// convert x, z camera frame values to mm space
pixel_in_camera_frame.x = (MM_TO_PIX_X_PLANE)*pixel_in_camera_frame.x;
pixel_in_camera_frame.z = (MM_TO_PIX_Y_PLANE)*pixel_in_camera_frame.z;
// declare x,y,z coord of pixel in relation to body center
point3 <float> pixel_in_body_frame;
// transform camera coordinates to body frame coordinates for a test pixel
pixel_in_body_frame = image_matrix.transform(pixel_in_camera_frame);
// find object z from body center (need to convert object_height to mms)
float object_z_in_body_frame = -body_center_height+(object_height*10);
// declare 3d line parametric multiplier 't' from object to body center (orig)
float object_to_origin_t = 0;
// calculate t knowing object_z_in_body_frame
if ((focal_point_body_frame.z-pixel_in_body_frame.z) != 0) {
object_to_origin_t = (object_z_in_body_frame-pixel_in_body_frame.z)/(focal_point_body_frame.z-pixel_in_body_frame.z);
}
// declare 3d coord of the object's xyz point in body frame
point3 <float> object_in_body_frame;
// calculate x,y,z using parametric form of 3d vector
object_in_body_frame.x = pixel_in_body_frame.x +
(focal_point_body_frame.x-pixel_in_body_frame.x)*object_to_origin_t;
object_in_body_frame.y = pixel_in_body_frame.y +
(focal_point_body_frame.y-pixel_in_body_frame.y)*object_to_origin_t;
object_in_body_frame.z = pixel_in_body_frame.z +
(focal_point_body_frame.z-pixel_in_body_frame.z)*object_to_origin_t;
// declare null estimate
estimate pix_est = {0,0,0,0,0};
// SANITY CHECKS
// if the object height > focal point and pixel > focal point in image plane,
if (object_height*10 < body_center_height+focal_point_body_frame.z && pixel_in_body_frame.z > focal_point_body_frame.z) {
return pix_est;
}
// calculate in degrees the bearing of the object from the center of the body
if (object_in_body_frame.x/object_in_body_frame.y != 0) {
pix_est.bearing = -RAD2DEG(atan(object_in_body_frame.x/object_in_body_frame.y));
}
// NEED TO CALCULATE ELEVATION
// calculate dist from body center in 3 dimensions
//pix_est.dist = get3dDist(object_in_body_frame, ZERO_COORD)/10.0;
// get dist from origin just on the plane of x/y
pix_est.dist = getHypotenuse(object_in_body_frame.x, object_in_body_frame.y)/10.0;
// Convert from mm to cm
pix_est.x = object_in_body_frame.x / 10;
pix_est.y = object_in_body_frame.y / 10;
/*//////////////////////////////////////////////////
Corrections based on curve fitting (use only one). -yz
The first one is the better one to uses.
Both are commented out for now, since the distance to corners were always overestimated and
there's code that handled this problem some where else. Right now, fixing the distance
estimation will cause the dog to back into the goal area whenever he sees the ball.
(he localizes himself correctly when he does not see the ball though)
need to change that piece of code before we can use this correction.
*/
// Yi's
//pix_est.dist = 2.86618 + 0.937351 * pix_est.dist - 0.0020501 * pix_est.dist * pix_est.dist;
// Harrison's regression
//pix_est.dist = .96962065 * pix_est.dist - 5.79841029;
/*pix_est.dist = 5.30471 + 0.723296 * pix_est.dist + 0.00124533 * (pix_est.dist * pix_est.dist);*/
/*pix_est.dist = 8.0977 + 0.692808 * pix_est.dist + 0.000428233 * (pix_est.dist * pix_est.dist);*/
pix_est.dist = -3.3893113 + 0.9859228*pix_est.dist+ -0.0007460*pix_est.dist*pix_est.dist;
// Yi's second one
//(2) pix_est.dist = 8.22999 + 0.879145*pix_est.dist - 0.00204601*pix_est.dist*pix_est.dist + 19.9248*yaw_angle ;
/////////////////////////////////////////////////-end of yz
return pix_est;
}
