C++ SearchController类代码示例

bool ScanUART::testBootMode(QSerialPort &port)
{
    bool foundFlag = false;
    SearchController contr;

    Request req;
    req.setNetAddress(0);

    port.setBaudRate(QSerialPort::Baud115200);
    CommandInterface* cmd = new GetCoreVersion();

    if(cmd->execute(req,port)){
        contr.setBaudrate(115200);
        if(QString(req.getRdData()).contains("boot")) contr.setBootMode(true);
        else contr.setBootMode(false);
        contr.setAsciiMode(false);
        contr.setNetAddress(0);
        contr.setUartName(pName);
        if(contr.getBootMode()) {
            emit plcHasBeenFound(contr);
            foundFlag = true;
        }
    }
    delete cmd;
    if(foundFlag) return true;
    return false;
}

bool ScanUART::scan(QSerialPort &port)
{
    bool foundFlag = false;
    SearchController contr;
    float stepWidth = 100.0/(baudTable.count()*2);
    float currentPercent=0;
    for(int i=0;i<baudTable.count();i++) {

        Request req;
        req.setNetAddress(progAddr);

        port.setBaudRate(baudTable.at(i));
        CommandInterface* cmdBin = new GetCoreVersion();
        CommandInterface* cmdAscii = new AsciiDecorator(cmdBin);
        QVector<CommandInterface*> cmdList {cmdBin, cmdAscii};
        foundFlag = false;
        foreach(CommandInterface* cmd, cmdList) {
            if(cmd->execute(req,port)){
                contr.setBaudrate(baudTable.at(i));
                if(QString(req.getRdData()).contains("Relkon")) contr.setBootMode(false);
                else contr.setBootMode(true);
                if(dynamic_cast<AsciiDecorator*>(cmd)) contr.setAsciiMode(true);
                else contr.setAsciiMode(false);
                GetCoreVersion* coreCmd = dynamic_cast<GetCoreVersion*>(cmdBin);
                if(coreCmd) contr.setNetAddress(coreCmd->getNetAddress());
                else contr.setNetAddress(0);
                contr.setUartName(pName);

                // get canal name
                if(contr.getBootMode()==false) {
                    CommandInterface* cmdGetName = new GetCanName();
                    if(contr.getAsciiMode()) cmdGetName = new AsciiDecorator(cmdGetName);
                    if(cmdGetName->execute(req,port)) {
                        contr.setCanName(QString(req.getRdData()).remove("Canal:"));
                    }
                    delete cmdGetName;
                }

                emit percentUpdate(100);
                emit plcHasBeenFound(contr);

                foundFlag = true;


                break;
            }
            currentPercent+=stepWidth;
            emit percentUpdate(currentPercent);

        }
        delete cmdAscii; // cmdBin удаляется декоратором
        mutex.lock();
        if(stopCmd) {mutex.unlock();break;}
        mutex.unlock();
        if(foundFlag) break;
    }
    if(foundFlag) return true;
    return false;
}

		virtual bool doWork()
		{
			bool ret;
			for (size_t i = 0; i < saves.size(); i++)
			{
				if (publish)
					ret = PublishSave(saves[i]);
				else
					ret = UnpublishSave(saves[i]);
				if (!ret)
				{
					std::stringstream error;
					if (publish) // uses html page so error message will be spam
						error << "Failed to publish [" << saves[i] << "], is this save yours?";
					else
						error << "Failed to unpublish [" << saves[i] << "]: " + Client::Ref().GetLastError();
					notifyError(error.str());
					c->Refresh();
					return false;
				}
				notifyProgress((float(i+1)/float(saves.size())*100));
			}
			c->Refresh();
			return true;
		}

		bool doWork() override
		{
			for (size_t i = 0; i < saves.size(); i++)
			{
				notifyStatus(String::Build("Deleting save [", saves[i], "] ..."));
				if (Client::Ref().DeleteSave(saves[i])!=RequestOkay)
				{
					notifyError(String::Build("Failed to delete [", saves[i], "]: ", Client::Ref().GetLastError()));
					c->Refresh();
					return false;
				}
				notifyProgress((float(i+1)/float(saves.size())*100));
			}
			c->Refresh();
			return true;
		}

void obstacleHandler(const std_msgs::UInt8::ConstPtr& message) {
    if ((!targetDetected || targetCollected) && (message->data > 0)) {
        // obstacle on right side
        if (message->data == 1) {
            // select new heading 0.2 radians to the left
            goalLocation.theta = currentLocation.theta + 0.6;
        }

        // obstacle in front or on left side
        else if (message->data == 2) {
            // select new heading 0.2 radians to the right
            goalLocation.theta = currentLocation.theta + 0.6;
        }

        // continues an interrupted search
        goalLocation = searchController.continueInterruptedSearch(currentLocation, goalLocation);

        // switch to transform state to trigger collision avoidance
        stateMachineState = STATE_MACHINE_ROTATE;

        avoidingObstacle = true;
    }

    // the front ultrasond is blocked very closely. 0.14m currently
    if (message->data == 4) {
        blockBlock = true;
    } else {
        blockBlock = false;
    }
}

		virtual void ConfirmCallback(ConfirmPrompt::DialogueResult result) {
			if (result == ConfirmPrompt::ResultOkay)
				c->unpublishSelectedC(publish);
		}

void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& message) {

    // If in manual mode do not try to automatically pick up the target
    if (currentMode == 1 || currentMode == 0) return;

    // if a target is detected and we are looking for center tags
    if (message->detections.size() > 0 && !reachedCollectionPoint) {
        float cameraOffsetCorrection = 0.020; //meters;

        centerSeen = false;
        double count = 0;
        double countRight = 0;
        double countLeft = 0;

        // this loop is to get the number of center tags
        for (int i = 0; i < message->detections.size(); i++) {
            if (message->detections[i].id == 256) {
                geometry_msgs::PoseStamped cenPose = message->detections[i].pose;

                // checks if tag is on the right or left side of the image
                if (cenPose.pose.position.x + cameraOffsetCorrection > 0) {
                    countRight++;

                } else {
                    countLeft++;
                }

                centerSeen = true;
                count++;
            }
        }

        if (centerSeen && targetCollected) {
            stateMachineState = STATE_MACHINE_TRANSFORM;
            goalLocation = currentLocation;
        }

        dropOffController.setDataTargets(count,countLeft,countRight);

        // if we see the center and we dont have a target collected
        if (centerSeen && !targetCollected) {

            float centeringTurn = 0.15; //radians
            stateMachineState = STATE_MACHINE_TRANSFORM;

            // this code keeps the robot from driving over
            // the center when searching for blocks
            if (right) {
                // turn away from the center to the left if just driving
                // around/searching.
                goalLocation.theta += centeringTurn;
            } else {
                // turn away from the center to the right if just driving
                // around/searching.
                goalLocation.theta -= centeringTurn;
            }

            // continues an interrupted search
            goalLocation = searchController.continueInterruptedSearch(currentLocation, goalLocation);

            targetDetected = false;
            pickUpController.reset();

            return;
        }
    }
    // end found target and looking for center tags

    // found a target april tag and looking for april cubes;
    // with safety timer at greater than 5 seconds.
    PickUpResult result;

    if (message->detections.size() > 0 && !targetCollected && timerTimeElapsed > 5) {
        targetDetected = true;

        // pickup state so target handler can take over driving.
        stateMachineState = STATE_MACHINE_PICKUP;
        result = pickUpController.selectTarget(message);

        std_msgs::Float32 angle;

        if (result.fingerAngle != -1) {
            angle.data = result.fingerAngle;
            fingerAnglePublish.publish(angle);
        }

        if (result.wristAngle != -1) {
            angle.data = result.wristAngle;
            wristAnglePublish.publish(angle);
        }
    }
}

// This is the top-most logic control block organised as a state machine.
// This function calls the dropOff, pickUp, and search controllers.
// This block passes the goal location to the proportional-integral-derivative
// controllers in the abridge package.
void mobilityStateMachine(const ros::TimerEvent&) {

    std_msgs::String stateMachineMsg;
    float rotateOnlyAngleTolerance = 0.4;
    int returnToSearchDelay = 5;

    // calls the averaging function, also responsible for
    // transform from Map frame to odom frame.
    mapAverage();

    // Robot is in automode
    if (currentMode == 2 || currentMode == 3) {


        // time since timerStartTime was set to current time
        timerTimeElapsed = time(0) - timerStartTime;

        // init code goes here. (code that runs only once at start of
        // auto mode but wont work in main goes here)
        if (!init) {
            if (timerTimeElapsed > startDelayInSeconds) {
                // Set the location of the center circle location in the map
                // frame based upon our current average location on the map.
                centerLocationMap.x = currentLocationAverage.x;
                centerLocationMap.y = currentLocationAverage.y;
                centerLocationMap.theta = currentLocationAverage.theta;

                // initialization has run
                init = true;
            } else {
                return;
            }

        }

        // If no collected or detected blocks set fingers
        // to open wide and raised position.
        if (!targetCollected && !targetDetected) {
            // set gripper
            std_msgs::Float32 angle;

            // open fingers
            angle.data = M_PI_2;

            fingerAnglePublish.publish(angle);
            angle.data = 0;

            // raise wrist
            wristAnglePublish.publish(angle);
        }

        // Select rotation or translation based on required adjustment
        switch(stateMachineState) {

        // If no adjustment needed, select new goal
        case STATE_MACHINE_TRANSFORM: {
            stateMachineMsg.data = "TRANSFORMING";

            // If returning with a target
            if (targetCollected && !avoidingObstacle) {
                // calculate the euclidean distance between
                // centerLocation and currentLocation
                dropOffController.setCenterDist(hypot(centerLocation.x - currentLocation.x, centerLocation.y - currentLocation.y));
                dropOffController.setDataLocations(centerLocation, currentLocation, timerTimeElapsed);

                DropOffResult result = dropOffController.getState();

                if (result.timer) {
                    timerStartTime = time(0);
                    reachedCollectionPoint = true;
                }

                std_msgs::Float32 angle;

                if (result.fingerAngle != -1) {
                    angle.data = result.fingerAngle;
                    fingerAnglePublish.publish(angle);
                }

                if (result.wristAngle != -1) {
                    angle.data = result.wristAngle;
                    wristAnglePublish.publish(angle);
                }

                if (result.reset) {
                    timerStartTime = time(0);
                    targetCollected = false;
                    targetDetected = false;
                    lockTarget = false;
                    sendDriveCommand(0.0,0);

                    // move back to transform step
                    stateMachineState = STATE_MACHINE_TRANSFORM;
                    reachedCollectionPoint = false;;
                    centerLocationOdom = currentLocation;

//.........这里部分代码省略.........