The Robot Superclass

The Robot superclass provides the baseline functionality for all robots in this simulation. Functionality shared by all robots include the ability to be visualised on Stage, dynamic pathfinding, and basic movement.

It is important to familiarise yourself with Robot before attempting to create any additional robots.

###Basic Functionality

void Robot::startSpinning(bool clockwise) {

    if (clockwise) {
        spin = CLOCKWISE;
    } else {
        spin = ANTI_CLOCKWISE;
    }
}

void Robot::stopSpinning() {

    spin = NOT_SPINNING;
}

You can call the above methods if you wish for your robot to spin. This is particularly useful as visual feedback for debugging/task acknowledgment.

###Dynamic Pathfinding

void Robot::goToLocation(const geometry_msgs::Point location) { 

    elderly_care_simulation::FindPath srv;
    srv.request.from_point = currentLocation.position;
    srv.request.to_point = location;
    if (pathFinderService.call(srv)) {
        clearLocationQueue();
        addPointsToQueue(srv.response.path);
    } else {
        ROS_INFO("Call failed");
    }

    finalDestination.x = location.x;
    finalDestination.y = location.y;
}

goToLocation() can be used to navigate a Robot from its current location to a new location. The Robot will cancel its current navigation task and replace it with this one.

/**
 * Returns true if the robot is close enough to the final
 * target's location and false otherwise. 
 */
bool Robot::atDesiredLocation() {  

    if (locationQueue.empty()) {
        return true;
    } else {
        double toleratedDifference = 0.05;
        geometry_msgs::Point desiredLocation = locationQueue.front();

        if (doubleEquals(currentLocation.position.x, desiredLocation.x, toleratedDifference) &&
            doubleEquals(currentLocation.position.y, desiredLocation.y, toleratedDifference)) {
            locationQueue.pop();
            return atDesiredLocation();
        }
    }
    return false;
      
}

Call atDesiredLocation() periodically during navigation to see if a Robot should stop and perform its task.

void Robot::updateCurrentVelocity() {

    if (spin == CLOCKWISE) {
        // Spin clockwise
        currentVelocity.linear.x = 0;
        currentVelocity.angular.z = M_PI / 4;
    } else if (spin == ANTI_CLOCKWISE) {
        // Spin anti_clockwise
        currentVelocity.linear.x = 0;
        currentVelocity.angular.z = - M_PI / 4;
    } else if (atDesiredLocation()) {
        // Stop robot
        currentVelocity.linear.x = 0;
        currentVelocity.angular.z = 0;
    } else {
        updateCurrentVelocityToDesiredLocation();
    }
    robotNodeStagePub.publish(currentVelocity);
}

At the end of the Robot's while loop, this method should be called. It will cause the Robot to stop if it is at its desired location, or change direction if necessary.