motorVels SwerveDrive::doVelTranslation(const geometry_msgs::Twist * velMsg) {
motorVels output;
double velMagnitude = getVelMagnitude(velMsg);
//santity check, if the magnitude is close to zero stop
if (velMagnitude < VEL_ERROR) {
output = stop(output);
//account for the special case where y=0
} else if(fabs(velMsg->linear.y) >= VEL_ERROR) {
const double turnAngle = atan2(velMsg->linear.y,velMsg->linear.x);
direction = getDir(velMsg->linear.x);
output = steer(output, velMagnitude, turnAngle);
} else {
ROS_INFO("drive straight");
output.frontLeftMotorV = output.backLeftMotorV = output.frontRightMotorV = output.backRightMotorV = velMsg->linear.x;
output.frontRightAng = output.frontLeftAng = 0;
}
return output;
}
geometry_msgs::Vector3
SSLPathPlanner::exePlanForRobot (const int& id)
{
geometry_msgs::Point curr_point = getCurrentPosition (id);
geometry_msgs::Point next_point;
next_point.x = next_target_poses[id].x;
next_point.y = next_target_poses[id].y;
next_point.z = 0.0;
geometry_msgs::Point target_point;
target_point.x = pose_control.pose[id].pose.x;
target_point.y = pose_control.pose[id].pose.y;
target_point.z = 0.0;
double curr_theta = team_state_[id].pose.theta;
// std::cout<<curr_theta<<std::endl;
double goal_theta = next_target_poses[id].theta;
// std::cout<<goal_theta<<std::endl;
double angular_diff = getAngularDifference (curr_theta, goal_theta);
// std::cout<<"angular_diff: "<<angular_diff/ssl::math::PI*180.0<<std::endl;
double angular_vel;
if (fabs (angular_diff) > ssl::math::PI / 2.0)
angular_vel = -MAX_ANGULAR_VEL * ssl::math::sign (angular_diff);
else
angular_vel = -MAX_ANGULAR_VEL * (angular_diff / (ssl::math::PI / 2.0));
// angular_vel = ssl::math::sign(angular_diff)*MAX_ANGULAR_VEL*(1-exp(-(fabs(angular_diff))));
markWaypoint (next_point, 0, id);
double dist = sqrt (getSquaredDistance (curr_point, next_point));
double ang = getAngleBetween (curr_point, next_point);
// if(dist<VERY_CRITICAL_DIST)
// {
// geometry_msgs::Vector3 v;
// double v_mag = dist/CRITICAL_DISTANCE * CRITICAL_LINEAR_VEL;
// v.x = v_mag * cos(goal_theta);
// v.y = v_mag * sin(goal_theta);
// return v;
// }
double vel_x = 0;
double vel_y = 0;
bool transit_pass = true;
if (next_point.x == target_point.x && next_point.y == target_point.y && next_point.z == target_point.z)
{
transit_pass = false;
}
if (dist > CRITICAL_DISTANCE)
{
vel_x = MAX_LINEAR_VEL * cos (ang);
vel_y = MAX_LINEAR_VEL * sin (ang);
//now consider obstacles around
}
else
{
if (!transit_pass)
{
if (dist < VERY_CRITICAL_DIST)
{
vel_x = CRITICAL_LINEAR_VEL * (dist / VERY_CRITICAL_DIST) * cos (ang);
vel_y = CRITICAL_LINEAR_VEL * (dist / VERY_CRITICAL_DIST) * sin (ang);
}
else
{
vel_x = MAX_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * cos (ang);
vel_y = MAX_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * sin (ang);
}
}
else
{
//TODO optimize this
vel_x = INT_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * cos (ang);
vel_y = INT_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * sin (ang);
}
}
geometry_msgs::Vector3 v = getCurrentVelocity (id);
// double curr_vel = getVelMagnitude(v);
// double curr_vel_ang= getVelAngle(v);
double des_acc_x = (vel_x - v.x) / ssl::config::TIME_STEP_SEC;
double des_acc_y = (vel_y - v.y) / ssl::config::TIME_STEP_SEC;
double des_acc = sqrt (des_acc_x * des_acc_x + des_acc_y * des_acc_y);
double des_acc_ang = atan2 (des_acc_y, des_acc_x);
if (des_acc > MAX_LINEAR_ACC)
des_acc = MAX_LINEAR_ACC;
des_acc_x = MAX_LINEAR_ACC * cos (des_acc_ang);
des_acc_y = MAX_LINEAR_ACC * sin (des_acc_ang);
v.x += des_acc_x * ssl::config::TIME_STEP_SEC * ACC_TUNING;
v.y += des_acc_y * ssl::config::TIME_STEP_SEC * ACC_TUNING;
double des_vel_ang = getVelAngle (v);
double des_vel_mag = getVelMagnitude (v);
if (des_vel_mag > MAX_LINEAR_VEL)
des_vel_mag = MAX_LINEAR_VEL;
v.x = des_vel_mag * cos (des_vel_ang);
v.y = des_vel_mag * sin (des_vel_ang);
tf::Vector3 tf_v;
tf::vector3MsgToTF (v, tf_v);
tf_v = tf_v.rotate (tf::Vector3 (0, 0, 1), -curr_theta);
tf::vector3TFToMsg (tf_v, v);
v.z = angular_vel;
return v;
}
