void InterceptCarefully::run(void* msg)
{
/*PROTECTED REGION ID(run1427703218101) ENABLED START*/ //Add additional options here
auto ownPos = this->wm->rawSensorData->getOwnPositionVision();
auto egoTarget = this->wm->ball->getEgoBallPosition();
if (ownPos == nullptr || egoTarget == nullptr)
{
return;
}
auto eval = make_shared<msl::PathEvaluator>();
shared_ptr < vector<shared_ptr<geometry::CNPoint2D>>> additionalPoints;
if (auto pathPlanningResult = msl::PathProxy::getInstance()->getEgoDirection(egoTarget, eval, additionalPoints))
{
egoTarget = pathPlanningResult;
}
msl_actuator_msgs::MotionControl mc;
mc.motion.angle = egoTarget->angleTo();
mc.motion.rotation = egoTarget->rotate(M_PI)->angleTo() * interceptCarfullyRotateP;
if (egoTarget->length() > this->catchRadius)
{
mc.motion.translation = min(defaultTranslation, egoTarget->length());
}
else
{
mc.motion.translation = 0;
}
send(mc);
/*PROTECTED REGION END*/
}
Quaternion Quaternion::slerp(const Quaternion &other, double by) const {
auto angle = angleTo(other);
if(std::fabs(angle) < Math::Constants::Epsilon) return other;
if(by > angle) by = angle;
double u = by / angle;
double a = std::sin((1 - u) * angle) / std::sin(angle);
double b = std::sin(u * angle) / std::sin(angle);
Quaternion result(m_s * a + other.m_s * b,
m_v * a + other.m_v * b);
double length = std::sqrt(result.m_s*result.m_s + result.m_v.length2());
result.m_s /= length;
result.m_v /= length;
return result;
}
void GoalieExtension::run(void* msg)
{
/*PROTECTED REGION ID(run1459249216387) ENABLED START*/ //Add additional options here
auto ownPos = wm->rawSensorData->getOwnPositionVision();
if (ownPos == nullptr)
return;
auto ballPos = wm->ball->getEgoBallPosition();
if (ballPos == nullptr)
return;
//kick
msl_actuator_msgs::KickControl km;
long currentTime = wm->getTime();
if (currentTime - lastKickerTime >= KICKER_WAIT_TIME)
{
if (useKicker == true && ballPos != nullptr && ballPos->length() < 420
&& (abs(ballPos->angleTo()) - M_PI) < 0.52)
{
km.enabled = true;
km.kicker = 1;
km.power = 100;
send(km);
lastKickerTime = wm->getTime();
}
}
if (wm->rawSensorData->getLastMotionCommand() == nullptr)
return;
bm_last = msl_actuator_msgs::MotionControl();
bm_last.motion.translation = wm->rawSensorData->getLastMotionCommand()->motion.translation;
bm_last.motion.rotation = wm->rawSensorData->getLastMotionCommand()->motion.rotation;
bm_last.motion.angle = wm->rawSensorData->getLastMotionCommand()->motion.angle;
auto ballPosAllo = ballPos->egoToAllo(*ownPos);
long now = wm->getTime() / 1000000;
auto ballPos3D = wm->ball->getBallPoint3D();
if (ballPos3D != nullptr)
{
if (ballPos3D->z > 500)
{
ballInAirTimestamp = now;
}
}
auto ballV3D = wm->ball->getBallVel3D();
if (ballV3D != nullptr && ballPos != nullptr)
{
auto ballV3DAllo = ballV3D->egoToAllo(*ownPos);
double velo = sqrt(ballV3D->x * ballV3D->x + ballV3D->y * ballV3D->y + ballV3D->z * ballV3D->z);
if (velo > 1000)
{
double diffAngle = abs(wm->rawSensorData->getLastMotionCommand()->motion.angle - bm_last.motion.angle);
double diffTrans = abs(
wm->rawSensorData->getLastMotionCommand()->motion.translation - bm_last.motion.translation);
double diffRot = abs(
wm->rawSensorData->getLastMotionCommand()->motion.rotation - bm_last.motion.rotation);
double distBall = ballPos->length();
double speed = wm->rawSensorData->getLastMotionCommand()->motion.translation;
double g = 0;
double g1 = 1.0 / (1.0 + exp(0.03 * (speed - 100.0))); //speed
double g2 = 1.0 / (1.0 + exp(10 * (diffAngle - 0.5)));
g2 = 1;
double g3 = 1.0 / (1.0 + exp(0.03 * (diffTrans - 100)));
g3 = 1;
double g4 = 1.0 / (1.0 + exp(10 * (diffRot - 100)));
double g5 = 1.0 / (1.0 + exp(0.0006 * (distBall - 6000)));
g = g1 * g2 * g3 * g4 * g5;
shared_ptr < geometry::CNPoint3D > ballVelo3DAllo = make_shared < geometry::CNPoint3D
> (ballV3DAllo->x, ballV3DAllo->y, ballV3DAllo->z);
double x = -wm->field->getFieldLength() / 2 + 100;
double timeBallToGoal = (x - ballPosAllo->x) / ballVelo3DAllo->x;
//Console.WriteLine("ghgh timeBallToGoal " + timeBallToGoal);
if (timeBallToGoal > 0)
{
double y = ballPosAllo->y + ballVelo3DAllo->y * timeBallToGoal;
if (abs(y) < wm->field->getGoalWidth() / 2 + 2000)
{
if (y > wm->field->getGoalWidth() / 2)
y = wm->field->getGoalWidth() / 2;
if (y < -wm->field->getGoalWidth() / 2)
y = -wm->field->getGoalWidth() / 2;
auto dstPoint = make_shared < geometry::CNPoint2D > (x, y);
auto dstPointEgo = dstPoint->alloToEgo(*ownPos);
auto lookAt = make_shared < geometry::CNPoint2D > (0, 0);
auto lookAtEgo = lookAt->alloToEgo(*ownPos);
double lookAtAngle = lookAtEgo->angleTo() + M_PI;
ballVelo3DAllo = ballVelo3DAllo->normalize();
ballGoalProjection->overWrite(dstPoint, g);
ballVelocity->overWrite(make_shared < geometry::CNPoint2D > (ballV3DAllo->x, ballV3DAllo->y),
g);
int count = (int)(ballPos->length() * ballPos->length()) / 1000000;
if (count > 3)
{
count = 3;
}
dstPoint = ballGoalProjection->getAvgPoint(count);
dstPointEgo = dstPoint->alloToEgo(*ownPos);
auto ballVeloBuf = ballVelocity->getAvgPoint((int)(ballPos->length() - 2000) / 2000);
double veloBuf = sqrt(ballVeloBuf->x * ballVeloBuf->x + ballVeloBuf->y * ballVeloBuf->y);
double timeBallToGoal2 = ballPosAllo->distanceTo(wm->field->posOwnGoalMid()) / veloBuf;
double timeToDstPoint = dstPointEgo->length() / 800;
if (timeToDstPoint > timeBallToGoal2 && ballPos->length() < 5000)
{
if (useExt3 == true && dstPointEgo->angleTo() < 0)
{
km.extension = msl_actuator_msgs::KickControl::LEFT_EXTENSION;
}
else if (useExt2 == true)
{
km.extension = msl_actuator_msgs::KickControl::RIGHT_EXTENSION;
}
if (useExt3 == true || useExt2 == true)
{
km.extTime = 1000;
send(km);
}
}
else if (useExt1 == true && timeBallToGoal2 < 1.0 && abs(dstPointEgo->y - ownPos->y) < 400
&& ballInAirTimestamp + 3000 > now)
{
km.extension = msl_actuator_msgs::KickControl::UPPER_EXTENSION;
km.extTime = 1000;
send(km);
}
}
}
}
}
/*PROTECTED REGION END*/
}
bool Vector::isParallelWith(Vector const& second)
{
double angle = angleTo(second);
return std::abs(angle - 0) <= libcity::EPSILON || std::abs(angle - libcity::PI) <= libcity::EPSILON;
}
void Robot::moveTo(Location location) {
Location target = getStageLocation(location);
player_color cyan;
cyan.red = 0;
cyan.green = 255;
cyan.blue = 255;
cyan.alpha = 255;
double biggestSize = getWidth() > getHeight() ? getWidthMeters() : getHeightMeters();
pp->SetMotorEnable(true);
pp->SetSpeed(0, 0);
double d;
while((d = distanceTo(target)) > biggestSize/2) {
pc->Read();
gp->Clear();
Position currentPosition(pp->GetXPos(), pp->GetYPos(), pp->GetYaw());
setPosition(currentPosition);
drawPoint(target, cyan);
player_point_2d points[2];
points[0].px = getX();
points[0].py = getY();
points[1].px = target.getX();
points[1].py = target.getY();
gp->Color(255, 0, 0, 255);
gp->DrawPolyline(points, 2);
double targetYaw = angleTo(target);
double yawDiff = targetYaw - getYaw();
while(yawDiff < dtor(180))
yawDiff += dtor(360);
while(yawDiff > dtor(180))
yawDiff -= dtor(360);
double speed;
if(abs(yawDiff) < 0.1)
speed = maxSpeed;
else if(abs(yawDiff) > dtor(45))
speed = 0;
else
speed = maxSpeed * (1 - (abs(yawDiff) / dtor(45)));
if(speed > maxSpeed)
speed = maxSpeed;
double turnSpeed = yawDiff;
if(abs(turnSpeed) > maxTurnSpeed)
turnSpeed = (turnSpeed/abs(turnSpeed))*maxTurnSpeed;
pp->SetSpeed(speed, turnSpeed);
}
pp->SetSpeed(0, 0);
pp->SetMotorEnable(false);
}
void IGV::runProgram ()
{
addVisibleLinesToMap ();
if (autonomousMode)
{
//cout << "current rotation: " << rotation << endl;
// still have goals to reach
if (env.waypoints.size () > 0){
// a route to a goal has been mapped out already
if (followingPath)
{
pathNode = pf.getCurPathNode ();
// check if current node is close enough to move on to next
float dist_to_node = position.distance (pathNode);
if (dist_to_node < pf.getGoalDistance () )
{
// cout << "Reached the next node in the path\n";
if (pf.path.size () > 1)
{
pf.setNextPathNode ();
// cout << "Setting the next path node\n";
// cout << "Nodes remaining: " << pf.path.size () << endl;
//pathNode = pf.getCurPathNode ();
}
// reached the end of current path to a waypoint
else
{
// cout << "Reached the waypoint, going on to the next\n";
fullStop ();
pf.clear ();
env.waypoints.pop_front();
followingPath = false;
}
}
else{
// align to next node in the path
float theta = angleTo (pathNode);
//cout << "angle to next node: " << theta << endl;
// check for a potential collision, if one might occur then the
// current path should be abandoned and a new one should be formed
vector <pair <vertex, Line*> > closeWalls = env.lineMap->getObjectsInRegion (pathNode - pf.getWallDistance (), pathNode + pf.getWallDistance () );
if (closeWalls.size () > 0){
fullStop ();
pf.clear ();
followingPath = false;
return;
}
// set a forward speed that is related to the angle to the next node
// as well as the distance to the next node
if (theta <= 45.0){
setRotateSpeed ( max ( 0.0, sin (theta/DEGREES_PER_RADIAN) ) );
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN) ) );
}
else if (theta >= 315.0){
setRotateSpeed ( max ( 0.0, -sin (theta/DEGREES_PER_RADIAN) ) );
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN) ) );
}
// turn left
else if (theta < 180.0){
setRotateSpeed (2.0);
//setForwardSpeed (0);
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN)/3.0 ) );
}
// turn right
else {
setRotateSpeed (-2.0);
//setForwardSpeed (0);
setForwardSpeed ( max ( 0.0, cos (theta/DEGREES_PER_RADIAN)/3.0 ) );
}
}
}
// generate a path to a goal
else
{
// start a new path
if ( !pf.searching () ){
findPath (env.waypoints.front());
}
// continue expanding on path search until a path is found or the
// specified waypoint is determined to be unreachable
else{
if ( !pf.done () ){
pf.expand ();
if (pf.pathImpossible ()){
pf.clear ();
env.waypoints.pop_front();
//cout << "Mission impossible~\n";
}
}
else{
followingPath = true;
}
}
}
}
}
}
/**
* Updates path logic.
*/
void PathHandler::updatePath()
{
geometry_msgs::Twist command;
publishState();
// no path? nothing to handle
if (getPathSize() == 0)
return;
// update our position if possible.
if (updateCurrentPosition() == false)
{
ROS_ERROR("Failed to update robot position.");
return;
}
if (mCurrentPathIndex < getPathSize() - 1) // we are moving along a line segment in the path
{
geometry_msgs::Point robotPos;
robotPos.x = mRobotPosition.getOrigin().x();
robotPos.y = mRobotPosition.getOrigin().y();
geometry_msgs::Point closestOnPath = closestPointOnLine(robotPos, mPath[mCurrentPathIndex].position, mPath[mCurrentPathIndex + 1].position);
//ROS_INFO("robotPos[%lf, %lf], closestOnPath[%lf, %lf]", robotPos.x, robotPos.y, closestOnPath.x, closestOnPath.y);
if (distanceBetweenPoints(closestOnPath, robotPos) > mResetDistanceTolerance)
{
ROS_INFO("Drove too far away from path, re-sending goal.");
mFollowState = FOLLOW_STATE_IDLE;
resendGoal();
clearPath();
return;
}
geometry_msgs::Point pointOnPath = getPointOnPathWithDist(closestOnPath, mLookForwardDistance);
//ROS_INFO("closestOnPath[%lf, %lf], pointOnPath[%lf, %lf]", closestOnPath.x, closestOnPath.y, pointOnPath.x, pointOnPath.y);
double angle = angleTo(pointOnPath);
/*double robotYaw = tf::getYaw(mRobotPosition.getRotation()); // only used for printing
ROS_INFO("Follow state: %d Robot pos: (%lf, %lf, %lf). Target pos: (%lf, %lf, %lf). RobotYaw: %lf. FocusYaw: %lf", mFollowState, mRobotPosition.getOrigin().getX(),
mRobotPosition.getOrigin().getY(), mRobotPosition.getOrigin().getZ(), pointOnPath.x,
pointOnPath.y, pointOnPath.z, robotYaw, angle);*/
//ROS_INFO("Angle to path: %f", angle);
command.linear.x = getScaledLinearSpeed(angle);
command.angular.z = getScaledAngularSpeed(angle);
if (distanceBetweenPoints(closestOnPath, mPath[mCurrentPathIndex + 1].position) < mDistanceTolerance)
{
//ROS_INFO("Moving to next line segment on path.");
mCurrentPathIndex++;
}
publishLocalPath(command.linear.x * 3, angle);
}
else // only rotate for final yaw
{
double yaw = tf::getYaw(mPath[getPathSize() - 1].orientation);
btVector3 orientation = btVector3(cos(yaw), sin(yaw), 0.0).rotate(btVector3(0,0,1), -tf::getYaw(mRobotPosition.getRotation()));
double angle = atan2(orientation.getY(), orientation.getX());
//ROS_INFO("Angle to final orientation: %f", angle);
if (fabs(angle) > mFinalYawTolerance)
command.angular.z = getScaledAngularSpeed(angle, true);
else
{
// path is done!
mFollowState = FOLLOW_STATE_FINISHED;
clearPath();
}
publishLocalPath(1.0, angle);
}
mCommandPub.publish(command);
}
Quaternion Vector3d::quaternionTo(const Vector3d& other) const
{
Vector3d axis = cross(other);
return axis.rotationAroundAxis(abs(angleTo(other)));
}