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 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 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 bool doWork()
{
for (size_t i = 0; i < saves.size(); i++)
{
std::stringstream saveID;
saveID << "Deleting save [" << saves[i] << "] ...";
notifyStatus(saveID.str());
if (Client::Ref().DeleteSave(saves[i])!=RequestOkay)
{
std::stringstream saveIDF;
saveIDF << "Failed to delete [" << saves[i] << "]: " << Client::Ref().GetLastError();
notifyError(saveIDF.str());
c->Refresh();
return false;
}
notifyProgress((float(i+1)/float(saves.size())*100));
}
c->Refresh();
return true;
}
bool doWork() override
{
bool ret;
for (size_t i = 0; i < saves.size(); i++)
{
if (publish)
ret = PublishSave(saves[i]);
else
ret = UnpublishSave(saves[i]);
if (!ret)
{
if (publish) // uses html page so error message will be spam
notifyError(String::Build("Failed to publish [", saves[i], "], is this save yours?"));
else
notifyError(String::Build("Failed to unpublish [", saves[i], "]: " + Client::Ref().GetLastError()));
c->Refresh();
return false;
}
notifyProgress((float(i+1)/float(saves.size())*100));
}
c->Refresh();
return true;
}
virtual void ConfirmCallback(ConfirmPrompt::DialogueResult result) {
if (result == ConfirmPrompt::ResultOkay)
c->unpublishSelectedC(publish);
}
virtual void ConfirmCallback(ConfirmPrompt::DialogueResult result) {
if (result == ConfirmPrompt::ResultOkay)
c->removeSelectedC();
}
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;
dropOffController.reset();
} else if (result.goalDriving && timerTimeElapsed >= 5 ) {
goalLocation = result.centerGoal;
stateMachineState = STATE_MACHINE_ROTATE;
timerStartTime = time(0);
}
// we are in precision/timed driving
else {
goalLocation = currentLocation;
sendDriveCommand(result.cmdVel,result.angleError);
stateMachineState = STATE_MACHINE_TRANSFORM;
break;
}
}
//If angle between current and goal is significant
//if error in heading is greater than 0.4 radians
else if (fabs(angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta)) > rotateOnlyAngleTolerance) {
stateMachineState = STATE_MACHINE_ROTATE;
}
//If goal has not yet been reached drive and maintane heading
else if (fabs(angles::shortest_angular_distance(currentLocation.theta, atan2(goalLocation.y - currentLocation.y, goalLocation.x - currentLocation.x))) < M_PI_2) {
stateMachineState = STATE_MACHINE_SKID_STEER;
}
//Otherwise, drop off target and select new random uniform heading
//If no targets have been detected, assign a new goal
else if (!targetDetected && timerTimeElapsed > returnToSearchDelay) {
goalLocation = searchController.search(currentLocation);
}
//Purposefully fall through to next case without breaking
}
// Calculate angle between currentLocation.theta and goalLocation.theta
// Rotate left or right depending on sign of angle
// Stay in this state until angle is minimized
case STATE_MACHINE_ROTATE: {
stateMachineMsg.data = "ROTATING";
// Calculate the diffrence between current and desired
// heading in radians.
float errorYaw = angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta);
// If angle > 0.4 radians rotate but dont drive forward.
if (fabs(angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta)) > rotateOnlyAngleTolerance) {
// rotate but dont drive 0.05 is to prevent turning in reverse
sendDriveCommand(0.05, errorYaw);
break;
} else {
// move to differential drive step
stateMachineState = STATE_MACHINE_SKID_STEER;
//fall through on purpose.
}
}
// Calculate angle between currentLocation.x/y and goalLocation.x/y
// Drive forward
// Stay in this state until angle is at least PI/2
case STATE_MACHINE_SKID_STEER: {
stateMachineMsg.data = "SKID_STEER";
// calculate the distance between current and desired heading in radians
float errorYaw = angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta);
// goal not yet reached drive while maintaining proper heading.
if (fabs(angles::shortest_angular_distance(currentLocation.theta, atan2(goalLocation.y - currentLocation.y, goalLocation.x - currentLocation.x))) < M_PI_2) {
// drive and turn simultaniously
sendDriveCommand(searchVelocity, errorYaw/2);
}
// goal is reached but desired heading is still wrong turn only
else if (fabs(angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta)) > 0.1) {
// rotate but dont drive
sendDriveCommand(0.0, errorYaw);
}
else {
// stop
sendDriveCommand(0.0, 0.0);
avoidingObstacle = false;
// move back to transform step
stateMachineState = STATE_MACHINE_TRANSFORM;
}
break;
}
case STATE_MACHINE_PICKUP: {
stateMachineMsg.data = "PICKUP";
PickUpResult result;
// we see a block and have not picked one up yet
if (targetDetected && !targetCollected) {
result = pickUpController.pickUpSelectedTarget(blockBlock);
sendDriveCommand(result.cmdVel,result.angleError);
std_msgs::Float32 angle;
if (result.fingerAngle != -1) {
angle.data = result.fingerAngle;
fingerAnglePublish.publish(angle);
}
if (result.wristAngle != -1) {
angle.data = result.wristAngle;
// raise wrist
wristAnglePublish.publish(angle);
}
if (result.giveUp) {
targetDetected = false;
stateMachineState = STATE_MACHINE_TRANSFORM;
sendDriveCommand(0,0);
pickUpController.reset();
}
if (result.pickedUp) {
pickUpController.reset();
// assume target has been picked up by gripper
targetCollected = true;
result.pickedUp = false;
stateMachineState = STATE_MACHINE_ROTATE;
goalLocation.theta = atan2(centerLocationOdom.y - currentLocation.y, centerLocationOdom.x - currentLocation.x);
// set center as goal position
goalLocation.x = centerLocationOdom.x = 0;
goalLocation.y = centerLocationOdom.y;
// lower wrist to avoid ultrasound sensors
std_msgs::Float32 angle;
angle.data = 0.8;
wristAnglePublish.publish(angle);
sendDriveCommand(0.0,0);
return;
}
} else {
stateMachineState = STATE_MACHINE_TRANSFORM;
}
break;
}
case STATE_MACHINE_DROPOFF: {
stateMachineMsg.data = "DROPOFF";
break;
}
default: {
break;
}
} /* end of switch() */
}
// mode is NOT auto
else {
// publish current state for the operator to see
stateMachineMsg.data = "WAITING";
}
// publish state machine string for user, only if it has changed, though
if (strcmp(stateMachineMsg.data.c_str(), prev_state_machine) != 0) {
stateMachinePublish.publish(stateMachineMsg);
sprintf(prev_state_machine, "%s", stateMachineMsg.data.c_str());
}
}
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;
}
