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*/
    }
Example #2
0
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*/
    }
Example #4
0
bool Vector::isParallelWith(Vector const& second)
{
  double angle = angleTo(second);
  return std::abs(angle - 0) <= libcity::EPSILON || std::abs(angle - libcity::PI) <= libcity::EPSILON;
}
Example #5
0
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);
}
Example #6
0
File: IGV.cpp Project: jrd730/AVS
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);
}
Example #8
0
Quaternion Vector3d::quaternionTo(const Vector3d& other) const
{
    Vector3d axis = cross(other);
    return axis.rotationAroundAxis(abs(angleTo(other)));
}