void UpdateWaypointDriver() { double thrust = 0; double steering = 0; // Are we in an execute state (e.g. received execute command and are // in ready state for driving). This part is not needed if you are // doing the Underwater Vehicle Competition, since you are not required to // drive to local waypoints. if(mpLocalWaypointListDriver->IsExecuting()) { // If the current element in the list is 0, then we have reached the // end of the list. The element ID in the list is set by the OCP using // the Execute message which the LocalWaypointListDriver service processes // for you. if(mpLocalWaypointListDriver->GetActiveListElementID() != 0) { // Get the current waypoint in the list. JAUS::SetLocalWaypoint wp = mpLocalWaypointListDriver->GetCurrentWaypoint(); JAUS::LocalPose pose = mpLocalPoseSensor->GetLocalPose(); // See if we reached the point. CxUtils::Point3D wpPos(wp.GetX(), wp.GetY(), wp.GetZ()); CxUtils::Point3D myPos(pose.GetX(), pose.GetY(), pose.GetZ()); double distanceToWaypoint = myPos.Distance(wpPos); double tolerance = 0.25; // See if the waypoint has a desired tolerance, if not use the // default value of 0.25 meter radius. if(wp.IsFieldPresent(JAUS::SetLocalWaypoint::PresenceVector::WaypointTolerance)) { tolerance = wp.GetWaypointTolerance(); } if(distanceToWaypoint <= tolerance) { // Reached point, advance list. mpLocalWaypointListDriver->AdvanceListElement(); } else { // Drive to local waypoint by generating a desired // thrust and steering. For this demo/example program // an open-loop control system is used. // Get angle to waypoint. double angle = atan2(wpPos.mY - myPos.mY, wpPos.mX - myPos.mX); double angleDiff = CxUtils::Orientation::AngleDiff(pose.GetYaw(), angle); // Steer to angle if(fabs(angleDiff) > CxUtils::CxToRadians(5)) { steering = 30; if(angleDiff < 0) { steering *= -1; } } else { // Within 5 degrees of desired heading // set thrust to 30 to drive, and // stop rotating. thrust = 30; steering = 0; } } } // Check for control of Primitive Driver component if(mLocalWaypointDriverComponent.AccessControlService()->HaveControl(mBaselineComponent.GetComponentID()) == false) { mLocalWaypointDriverComponent.AccessControlService()->RequestComponentControl(mBaselineComponent.GetComponentID()); // Put component in ready state. mLocalWaypointDriverComponent.ManagementService()->Resume(mBaselineComponent.GetComponentID()); } if(mLocalWaypointDriverComponent.AccessControlService()->HaveControl(mBaselineComponent.GetComponentID())) { // Send drive command to make robot move! JAUS::SetWrenchEffort wrenchCommand(mBaselineComponent.GetComponentID(), mLocalWaypointDriverComponent.GetComponentID()); wrenchCommand.SetPropulsiveLinearEffortX(thrust); wrenchCommand.SetPropulsiveRotationalEffortZ(steering); mLocalWaypointDriverComponent.Send(&wrenchCommand); } } else { // Make sure we release control if we don't need it. if(mLocalWaypointDriverComponent.AccessControlService()->HaveControl(mBaselineComponent.GetComponentID())) { mLocalWaypointDriverComponent.AccessControlService()->ReleaseComponentControl(mBaselineComponent.GetComponentID()); } } }