TrajectoryManager.cpp

Go to the documentation of this file.

#include "robodyn_controllers/TrajectoryManager.h"

TrajectoryManager::TrajectoryManager(const std::string& name)
    : JointTrajectoryManager()
    , GoalStatusSender(name)
    , velocityFactor(1.)
    , stopVelocityLimit(0.4)
    , stopAccelerationLimit(1.5)
    , jointSettingsValid(false)
    , poseSettingsValid(false)
    , basesValid(false)
    , zeroLimit(1e-5)
{
}

TrajectoryManager::~TrajectoryManager()
{
}

void TrajectoryManager::initialize_sim(const std::string& urdfDesc, double timeStepIn)
{
    timeStep = timeStepIn;
    boost::shared_ptr<JointPositionLimiter> posLimiter(new JointPositionLimiter);
    positionLimiter.reset(new JointNamePositionLimiter);
    positionLimiter->setJointPositionLimiter(posLimiter);
    treeIkPtr.reset(new MobileTreeIk);
    forceController.reset(new Cartesian_HybCntrl);
    treeIkPtr->loadFromParam(urdfDesc);
    treeIkPtr->setPositionLimiter(positionLimiter);
    jointTrajFactory.reset(new jointTrajFactory_type);
    cartTrajFactory.reset(new cartTrajFactory_type);
    rosJointTrajFactory.setFactory(jointTrajFactory);
    rosJointTrajFactory.setPositionLimiter(positionLimiter);
    rosCartTrajFactory.setFactory(cartTrajFactory);
    rosTreeIkTrajFactory.setCartesianFactory(cartTrajFactory);
    rosTreeIkTrajFactory.setTreeIk(treeIkPtr);
    rosForceTrajFactory.setCartesianFactory(cartTrajFactory);
    rosForceTrajFactory.setTreeIk(treeIkPtr);
    rosForceTrajFactory.setForceController(forceController);
}


void TrajectoryManager::initialize(const std::string& urdfFile, double timeStepIn)
{
    timeStep = timeStepIn;
    boost::shared_ptr<JointPositionLimiter> posLimiter(new JointPositionLimiter);
    positionLimiter.reset(new JointNamePositionLimiter);
    positionLimiter->setJointPositionLimiter(posLimiter);
    treeIkPtr.reset(new MobileTreeIk);
    forceController.reset(new Cartesian_HybCntrl);
    treeIkPtr->loadFromFile(urdfFile);
    treeIkPtr->setPositionLimiter(positionLimiter);
    treeIkPtr->setTimeStep(timeStep);
    treeIkPtr->setMaxJointDecel(stopAccelerationLimit);
    jointTrajFactory.reset(new jointTrajFactory_type);
    cartTrajFactory.reset(new cartTrajFactory_type);
    rosJointTrajFactory.setFactory(jointTrajFactory);
    rosJointTrajFactory.setPositionLimiter(positionLimiter);
    rosCartTrajFactory.setFactory(cartTrajFactory);
    rosTreeIkTrajFactory.setCartesianFactory(cartTrajFactory);
    rosTreeIkTrajFactory.setTreeIk(treeIkPtr);
    rosForceTrajFactory.setCartesianFactory(cartTrajFactory);
    rosForceTrajFactory.setTreeIk(treeIkPtr);
    rosForceTrajFactory.setForceController(forceController);
}

void TrajectoryManager::updateTree(const r2_msgs::ToolFrame& toolFrameMsg)
{
    toolFrameManager.updateToolFrames(toolFrameMsg, *treeIkPtr);
    treeIkPtr->resetSolver();
}

void TrajectoryManager::updateTrajectories(const r2_msgs::JointCommand& defaultCommandMsg, const r2_msgs::JointControlDataArray& defaultControlMsg, const sensor_msgs::JointState& startJointPositions, const sensor_msgs::JointState& startJointVels)
{
    setupJointState(defaultCommandMsg);
    setupJointControl(defaultControlMsg);

    // handle followers
    bool                       sendUpdate = false;
    sensor_msgs::JointState    nextPoint;
    ros::Duration              timeFromStart;
    actionlib_msgs::GoalStatus followerStatus;
    bool                       success;
    std::set<std::string>      kasquishJoints;

    for (GoalFollowerMap_type::iterator followerIt = goalFollowerMap.begin(); followerIt != goalFollowerMap.end();)
    {
        success = true;
        try
        {
            followerIt->second->getNextPoint(nextPoint, timeFromStart);
        }
        catch (std::exception& e)
        {
            // get next point failed
            followerIt->second->goalManager.setReadyState(false);
            followerIt->second->goalManager.setStatus(actionlib_msgs::GoalStatus::ABORTED);
            followerIt->second->goalManager.setText(e.what());
            success = false;
            // kasquish the joints
            const std::vector<std::string>& jointNames = followerIt->second->getJointNames();
            kasquishJoints.insert(jointNames.begin(), jointNames.end());
        }
        if (success)
        {
            unsigned int i;
            GoalReplanMap_type::iterator replanIt = goalReplanMap.find(followerIt->first);

            for (i = 0; i < nextPoint.name.size(); ++i)
            {
                std::map<std::string, unsigned int>::const_iterator jntIt = jointIndexMap.find(nextPoint.name[i]);

                if (jntIt != jointIndexMap.end())
                {
                    jointStateOut.position.at(jointIndexMap.at(nextPoint.name[i])) = nextPoint.position.at(i);
                    jointStateOut.velocity.at(jointIndexMap.at(nextPoint.name[i])) = nextPoint.velocity.at(i);
                    jointStateOut.effort.at(jointIndexMap.at(nextPoint.name[i])) = nextPoint.effort.at(i);

                    // check replan
                    if (!(replanIt == goalReplanMap.end() || replanIt->second.empty() || replanIt->second.at(i).type != r2_msgs::ReplanType::PAUSE))
                    {
                        // want to replan...can we?
                        if (controllableJoints.at(jntIt->first))
                        {
                            // we can so set to multiloop step
                            std::vector<std::string>::iterator controlIt = std::find(jointControlOut.joint.begin(), jointControlOut.joint.end(), jntIt->first);

                            if (controlIt == jointControlOut.joint.end())
                            {
                                // not there, add
                                jointControlOut.joint.push_back(jntIt->first);
                                jointControlOut.data.push_back(r2_msgs::JointControlData());
                                jointControlOut.data.back().commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
                            }
                            else
                            {
                                // found it
                                jointControlOut.data.at(controlIt - jointControlOut.joint.begin()).commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
                            }
                        }
                    }
                }
            }

            if (replanIt != goalReplanMap.end())
            {
                // only apply on first point so we just delete it after one run
                goalReplanMap.erase(replanIt);
            }
        }

        followerStatus = followerIt->second->goalManager.getStatus();

        // Store the current goalStatus for this follower
        std::pair<GoalStatusMap_type::iterator, bool> insRet = goalStatusMap.insert(std::make_pair(followerStatus.goal_id, followerStatus));

        if (insRet.second)
        {
            std::stringstream ss;
            ss << "First status " << (int)followerStatus.status
               << " for " << followerStatus.goal_id.id << ", " << followerStatus.goal_id.stamp;
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());
            addStatus(followerStatus);
            sendUpdate = true;
        }
        else if (insRet.first->second.status != followerStatus.status
                 || insRet.first->second.text != followerStatus.text)
        {
            std::stringstream ss;
            ss << "Status change to " << (int)followerStatus.status
               << " for " << followerStatus.goal_id.id << ", " << followerStatus.goal_id.stamp;
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());
            insRet.first->second = followerStatus;
            addStatus(followerStatus);
            sendUpdate = true;
        }

        // Process completion
        if (followerStatus.status == actionlib_msgs::GoalStatus::SUCCEEDED
                || followerStatus.status == actionlib_msgs::GoalStatus::ABORTED
                || followerStatus.status == actionlib_msgs::GoalStatus::PREEMPTED
                || followerStatus.status == actionlib_msgs::GoalStatus::REJECTED)
        {
            std::stringstream ss;
            ss << "Goal complete: " << (int)followerStatus.status
               << " for " << followerStatus.goal_id.id << ", " << followerStatus.goal_id.stamp;
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());
            goalStatusMap.erase(followerStatus.goal_id);
            followerIt = goalFollowerMap.erase(followerIt);
            goalReplanMap.erase(followerStatus.goal_id);
            removeFromJointGoalMap(followerStatus.goal_id);
        }
        else
        {
            ++followerIt;

            if (followerStatus.status != actionlib_msgs::GoalStatus::ACTIVE
                    && followerStatus.status != actionlib_msgs::GoalStatus::PENDING)
            {
                std::stringstream ss;
                ss << "Received uexpected status: " << (int)followerStatus.status
                   << " for " << followerStatus.goal_id.id << ", " << followerStatus.goal_id.stamp;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
            }
        }
    }

    // the joints in here should have been removed above so we just call kasquish here
    // if it fails, they will stop abruptly
    if (!kasquishJoints.empty())
    {
        std::vector<std::string> kasquishVec(kasquishJoints.size());
        std::copy(kasquishJoints.begin(), kasquishJoints.end(), kasquishVec.begin());
        kasquish(kasquishVec, startJointPositions, startJointVels);
    }

    for (unsigned int i = 0; i < jointStateOut.name.size(); ++i)
    {
        if (positionLimiter->hasLimits(jointStateOut.name[i]) &&
                positionLimiter->limit(jointStateOut.name[i], jointStateOut.position[i]))
        {
            jointStateOut.velocity[i] = 0.;
            jointStateOut.effort[i]   = 0.;
            std::stringstream ss;
            ss << "Position out limited for " << jointStateOut.name[i];
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::DEBUG, ss.str());
        }

        if (fabs(jointStateOut.velocity[i]) < zeroLimit)
        {
            jointStateOut.velocity[i] = 0;
        }
    }

    jointStateOut.header.stamp = ros::Time::now();
    writeJointState(jointStateOut);

    if (!jointControlOut.joint.empty())
    {
        jointControlOut.header.stamp = ros::Time::now();
        writeJointControl(jointControlOut);

        // clean up
        for (unsigned int i = 0; i < jointControlOut.joint.size();)
        {
            if (jointControlOut.data.at(i).commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP)
            {
                // tell it to reset to smooth next time around
                jointControlOut.data.at(i).commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
                ++i;
            }
            else
            {
                // remove
                jointControlOut.joint.erase(jointControlOut.joint.begin() + i);
                jointControlOut.data.erase(jointControlOut.data.begin() + i);
            }
        }
    }

    // Publish the goalStatuses
    if (sendUpdate)
    {
        sendStatusMessage();
        clearStatusMessage();
    }
}

void TrajectoryManager::setJointSettings(const r2_msgs::ControllerJointSettings& settingsMsg)
{
    std::stringstream ss;
    ss << "received new joint settings";
    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

    // Apply the settings
    try
    {
        rosJointTrajFactory.setSettings(settingsMsg);
        rosJointTrajFactory.getSettings(jointSettings);
        jointSettingsValid = true;
    }
    catch (std::exception& e)
    {
        jointSettingsValid = false;
    }
}

void TrajectoryManager::getJointSettings(r2_msgs::ControllerJointSettings& settingsMsg)
{
    rosJointTrajFactory.getSettings(settingsMsg);
}

void TrajectoryManager::setPoseSettings(const r2_msgs::ControllerPoseSettings& settingsMsg)
{
    std::stringstream ss;
    ss << "received new pose settings";
    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

    // Apply the settings
    try
    {
        rosCartTrajFactory.setSettings(settingsMsg);
        forceController->setPoseSettings(settingsMsg.maxLinearVelocity, settingsMsg.maxRotationalVelocity, settingsMsg.maxLinearAcceleration, settingsMsg.maxRotationalAcceleration);
        poseSettingsValid = true;
    }
    catch (std::exception& e)
    {
        poseSettingsValid = false;
    }
}

void TrajectoryManager::setJointCapabilities(const r2_msgs::JointCapability& capabilitiesMsg)
{
    positionLimiter->setLimits(capabilitiesMsg.name, capabilitiesMsg.positionLimitMin, capabilitiesMsg.positionLimitMax, capabilitiesMsg.velocityLimit);
}

void TrajectoryManager::setBases(const r2_msgs::StringArray& basesMsg)
{
    std::stringstream ss;
    ss << "received new bases:";

    for (unsigned int i = 0; i < basesMsg.data.size(); ++i)
    {
        ss << basesMsg.data[i] << " ";
    }

    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

    // Apply the settings
    try
    {
        treeIkPtr->setBases(basesMsg.data);
        basesValid = true;
    }
    catch (std::exception& e)
    {
        basesValid = false;
    }
}

void TrajectoryManager::updateInertia(const sensor_msgs::JointState& inertiaIn)
{
    if (inertiaIn.position.size() != inertiaIn.name.size())
    {
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, "inertia names and positions are not the same size");
    }

    // Apply the settings
    try
    {
        RosMsgConverter::JointStateToJntMap(inertiaIn, jointInertiaMap);
    }
    catch (std::exception& e)
    {
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, e.what());
        return;
    }

    treeIkPtr->setJointInertias(jointInertiaMap);
}

void TrajectoryManager::setPriorityTol(std::vector<double> priorityNum, std::vector<double> priorityLinearTol, std::vector<double> priorityAngularTol)
{
    std::map<int, std::pair<double, double> > priorityTolMap;

    if (priorityNum.size() != priorityLinearTol.size() || priorityNum.size() != priorityAngularTol.size())
    {
        std::stringstream err;
        err <<  "priorityNum, priorityLinearTol, and priorityAngularTol must all be the same size!";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, err.str());
        throw std::length_error(err.str());
    }

    for (unsigned int i = 0; i < priorityNum.size(); ++i)
    {
        priorityTolMap[(int)priorityNum[i]] = std::make_pair(priorityLinearTol[i], priorityAngularTol[i]);
    }

    treeIkPtr->setPriorityTol(priorityTolMap);
}

void TrajectoryManager::setIkParameters(double mBar, double kr, unsigned int maxIts, double maxTwist)
{
    treeIkPtr->setMBar(mBar);
    treeIkPtr->setKr(kr);
    treeIkPtr->setMaxCriticalIterations(maxIts);
    treeIkPtr->setMaxTwist(maxTwist);
}

void TrajectoryManager::setSensorNameMap(std::vector<std::string> sensorKeys, std::vector<std::string> sensorNames)
{
    std::map<std::string, std::string> sensorNameMap;

    if (sensorKeys.size() != sensorNames.size())
    {
        std::stringstream err;
        err <<  "sensor keys and sensor names must be the same size!";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, err.str());
        throw std::length_error(err.str());
    }

    for (unsigned int i = 0; i < sensorKeys.size(); ++i)
    {
        sensorNameMap[sensorKeys[i]] = sensorNames[i];
    }

    forceController->setSensorNameMap(sensorNameMap);
}

void TrajectoryManager::setForceParameters(struct ForceGains new_gains)
{
    forceController->setFilter(new_gains.filterAlpha);
    forceController->setGain(new_gains.forceGain, new_gains.torqueGain, new_gains.forceIntegral, new_gains.torqueIntegral,
      new_gains.forceDamping, new_gains.torqueDamping, new_gains.forceWindupLimit, new_gains.torqueWindupLimit,
      new_gains.positionErrorLimit, new_gains.rotationErrorLimit);
    forceController->doFeedForward = new_gains.doFeedForward;
}

void TrajectoryManager::updateSensorForces(const r2_msgs::WrenchState& wrenchSensors)
{
    if (wrenchSensors.name.size() != wrenchSensors.wrench.size())
    {
        std::stringstream ss;
        ss << "TrajectoryManager::updateForceSensors: sensor names and values do not match!";
        NasaCommonLogging::Logger::log("gov.nasa.controller.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
        throw (std::runtime_error(ss.str()));
    }

    for (unsigned int i = 0; i < wrenchSensors.name.size(); ++i)
    {
        forceSensorMap[wrenchSensors.name[i]] = KDL::Wrench(KDL::Vector(wrenchSensors.wrench[i].force.x, wrenchSensors.wrench[i].force.y, wrenchSensors.wrench[i].force.z),
                                                KDL::Vector(wrenchSensors.wrench[i].torque.x, wrenchSensors.wrench[i].torque.y, wrenchSensors.wrench[i].torque.z));
    }

    forceController->updateSensorForces(forceSensorMap);
}

void TrajectoryManager::updateActualPoseState(const r2_msgs::PoseState& actualPoseState)
{
    if (actualPoseState.name.size() != actualPoseState.positions.size() || actualPoseState.name.size() != actualPoseState.velocities.size())
    {
        std::stringstream ss;
        ss << "TrajectoryManager::updateForceSensors: actual pose state names and values do not match!";
        NasaCommonLogging::Logger::log("gov.nasa.controller.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
        throw (std::runtime_error(ss.str()));
    }

    if (actualPoseState.name != actualFrameNames)
    {
        actualFrameVelMap.clear();
        actualFrameNames = actualPoseState.name;
    }

    RosMsgConverter::PoseStateToFrameVelMap(actualPoseState, treeIkPtr->getBaseName(), actualFrameVelMap);
    //forceController->updateActualVelocity(actualFrameVelMap);
}

double TrajectoryManager::addForceWaypoints(const r2_msgs::PoseTrajectory& trajectory,
        const sensor_msgs::JointState& startJointPositions,
        const sensor_msgs::JointState& startJointVels,
        const sensor_msgs::JointState& prevJointVels,
        const r2_msgs::PoseState& startPoseState,
        const r2_msgs::PoseState& startPoseVels,
        const r2_msgs::ForceControlAxisArray& forceAxes)
{
    std::stringstream ss;
    ss << "TrajectoryManager::" << "received force waypoints: \n" << trajectory;
    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

    if (!poseSettingsValid)
    {
        // send message and stop
        std::stringstream ss;
        ss << "position settings invalid";
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
    }
    else if (!basesValid)
    {
        // send message and stop
        std::stringstream ss;
        ss << "bases invalid";
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
    }
    else if (forceAxes.axes.size() != forceAxes.nodes.size())
    {
        // send message and stop
        std::stringstream ss;
        ss << "forces invalid";
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
    }
    else
    {
        actionlib_msgs::GoalID goalId;
        goalId.stamp = trajectory.header.stamp;
        goalId.id    = trajectory.header.frame_id;

        std::set<std::string> kasquishJoints;
        abort(goalId, kasquishJoints);

        // get online trajectory
        treeIkPtr->resetMobileJoints();
        boost::shared_ptr<RosMsgJointTrajectory> onlineTraj;

        try
        {
            onlineTraj = rosForceTrajFactory.getTrajectory(startJointPositions, startJointVels, prevJointVels, startPoseState, startPoseVels, trajectory, forceAxes);
            preempt(onlineTraj->getJointNames(), kasquishJoints);

            if (kasquishJoints.empty())
            {
                std::auto_ptr<OnlineJointTrajectoryFollower> newFollower(new OnlineJointTrajectoryFollower);
                newFollower->setTrajectory(onlineTraj, timeStep, velocityFactor);

                goalFollowerMap.insert(goalId, newFollower);
                //        goalReplanMap.insert(goalId);
                sendStatusMessage(goalFollowerMap.at(goalId).goalManager.getStatus(), true);
                goalStatusMap.insert(std::make_pair(goalId, goalFollowerMap.at(goalId).goalManager.getStatus()));
                // Add the new joints to the jointGoalMap
                addToJointGoalMap(onlineTraj->getJointNames(), goalId);
            }
        }
        catch (std::exception& e)
        {
            // send message and stop
            sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, e.what(), true);
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, e.what());
        }

        if (!kasquishJoints.empty())
        {
            std::vector<std::string> kasquishVec(kasquishJoints.size());
            std::copy(kasquishJoints.begin(), kasquishJoints.end(), kasquishVec.begin());
            kasquish(kasquishVec, startJointPositions, startJointVels);

            // report the time to wait to try again
            double maxTime = 0.;

            for (std::vector<std::string>::const_iterator jointIt = kasquishVec.begin(); jointIt != kasquishVec.end(); ++jointIt)
            {
                JointGoalMap_type::const_iterator goalIt = jointGoalMap.find(*jointIt);

                if (goalIt != jointGoalMap.end())
                {
                    GoalFollowerMap_type::const_iterator followerIt = goalFollowerMap.find(goalIt->second);

                    if (followerIt != goalFollowerMap.end())
                    {
                        if (followerIt->second->getDuration() > maxTime)
                        {
                            maxTime = followerIt->second->getDuration();
                        }
                    }
                }
            }

            ss.str("");
            ss << "TrajectoryManager:: kaquish not empty, maxTime: " << maxTime;
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());
            return maxTime;
        }
    }

    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, "finished addForceWaypoints");
    return 0.;
}

double TrajectoryManager::addCartesianWaypoints(const r2_msgs::PoseTrajectory& trajectory,
        const sensor_msgs::JointState&        startJointPositions,
        const sensor_msgs::JointState&        startJointVels,
        const sensor_msgs::JointState&        prevJointVels,
        const r2_msgs::PoseState& startPoseState,
        const r2_msgs::PoseState& startPoseVels)
{
    std::stringstream ss;
    ss << "received cartesian waypoints: \n" << trajectory;
    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

    if (!poseSettingsValid)
    {
        // send message and stop
        std::stringstream ss;
        ss << "position settings invalid";
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
    }
    else if (!basesValid)
    {
        // send message and stop
        std::stringstream ss;
        ss << "bases invalid";
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
    }
    else
    {
        actionlib_msgs::GoalID goalId;
        goalId.stamp = trajectory.header.stamp;
        goalId.id    = trajectory.header.frame_id;

        std::set<std::string> kasquishJoints;
        abort(goalId, kasquishJoints, "duplicate goal");

        // get online trajectory
        treeIkPtr->resetMobileJoints();
        boost::shared_ptr<RosMsgJointTrajectory> onlineTraj;

        try
        {
            onlineTraj = rosTreeIkTrajFactory.getTrajectory(startJointPositions, startJointVels, prevJointVels, startPoseState, startPoseVels, trajectory);
            preempt(onlineTraj->getJointNames(), kasquishJoints, "new Cartesian waypoints");

            if (kasquishJoints.empty())
            {
                std::auto_ptr<OnlineJointTrajectoryFollower> newFollower(new OnlineJointTrajectoryFollower);
                newFollower->setTrajectory(onlineTraj, timeStep, velocityFactor);

                goalFollowerMap.insert(goalId, newFollower);
                sendStatusMessage(goalFollowerMap.at(goalId).goalManager.getStatus(), true);
                goalStatusMap.insert(std::make_pair(goalId, goalFollowerMap.at(goalId).goalManager.getStatus()));

                // Add the new joints to the jointGoalMap
                addToJointGoalMap(onlineTraj->getJointNames(), goalId);
            }
        }
        catch (std::exception& e)
        {
            // send message and stop
            sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, e.what(), true);
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, "error in addCartesianWaypoints()");
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, e.what());
        }

        if (!kasquishJoints.empty())
        {
            std::vector<std::string> kasquishVec(kasquishJoints.size());
            std::copy(kasquishJoints.begin(), kasquishJoints.end(), kasquishVec.begin());
            kasquish(kasquishVec, startJointPositions, startJointVels);

            // report the time to wait to try again
            double maxTime = timeStep;

            for (std::vector<std::string>::const_iterator jointIt = kasquishVec.begin(); jointIt != kasquishVec.end(); ++jointIt)
            {
                JointGoalMap_type::const_iterator goalIt = jointGoalMap.find(*jointIt);

                if (goalIt != jointGoalMap.end())
                {
                    GoalFollowerMap_type::const_iterator followerIt = goalFollowerMap.find(goalIt->second);

                    if (followerIt != goalFollowerMap.end())
                    {
                        if (followerIt->second->getDuration() > maxTime)
                        {
                            maxTime = followerIt->second->getDuration();
                        }
                    }
                }
            }

            return maxTime;
        }
    }

    return 0.;
}

void TrajectoryManager::addJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
        const sensor_msgs::JointState& startJointPositions,
        const sensor_msgs::JointState& startJointVels,
        const sensor_msgs::JointState& prevJointVels,
        const std::vector<r2_msgs::ReplanType>& replanVec)
{
    std::stringstream ss;
    ss << "received joint waypoints: \n" << trajectory;
    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

    // If no joint names are included in the trajectory, then stop all followers
    if (trajectory.joint_names.size() == 0)
    {
        std::stringstream ss;
        ss << "The new joint refs contains no joint names, resetAll";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::WARN, ss.str());
        resetAll("new joint waypoints with no joint names");
    }
    else if (!jointSettingsValid)
    {
        // send message and stop
        std::stringstream ss;
        ss << "joint settings invalid";
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, ss.str());
    }
    else
    {
        actionlib_msgs::GoalID goalId;
        goalId.stamp = trajectory.header.stamp;
        goalId.id    = trajectory.header.frame_id;
        std::set<std::string> kasquishJoints;
        abort(goalId, kasquishJoints, "duplicate goal");

        boost::shared_ptr<RosMsgJointTrajectory> onlineTraj;

        // get online trajectory
        try
        {
            onlineTraj = rosJointTrajFactory.getTrajectory(startJointPositions, startJointVels, prevJointVels, trajectory);

            const std::vector<std::string>& trajJoints = onlineTraj->getJointNames();
            preempt(trajJoints, kasquishJoints, "new joint waypoints");

            // remove trajectory joints from kasquish list
            for (std::vector<std::string>::const_iterator jointIt = trajJoints.begin(); jointIt != trajJoints.end(); ++jointIt)
            {
                kasquishJoints.erase(*jointIt);
            }

            std::auto_ptr<OnlineJointTrajectoryFollower> newFollower(new OnlineJointTrajectoryFollower);
            newFollower->setTrajectory(onlineTraj, timeStep, velocityFactor);

            goalFollowerMap.insert(goalId, newFollower);
            goalReplanMap[goalId] = replanVec;
            sendStatusMessage(goalFollowerMap.at(goalId).goalManager.getStatus(), true);
            goalStatusMap.insert(std::make_pair(goalId, goalFollowerMap.at(goalId).goalManager.getStatus()));

            // Add the new joints to the jointGoalMap
            addToJointGoalMap(onlineTraj->getJointNames(), goalId);
        }
        catch (std::exception& e)
        {
            // send message and stop
            sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, e.what(), true);
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, "error in addJointWaypoints()");
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, e.what());
        }

        if (!kasquishJoints.empty())
        {
            std::vector<std::string> kasquishVec(kasquishJoints.size());
            std::copy(kasquishJoints.begin(), kasquishJoints.end(), kasquishVec.begin());
            kasquish(kasquishVec, startJointPositions, startJointVels);
        }
    }
}

void TrajectoryManager::addJointBreadcrumbs(const trajectory_msgs::JointTrajectory& trajectory)
{
    std::stringstream ss;
    ss << "received trajectory: " << trajectory.header.frame_id << ", " << trajectory.points.size() << " points";

    for (unsigned int i = 0; i < trajectory.joint_names.size(); i++)
    {
        ss << std::endl << "    " << trajectory.joint_names[i];
    }

    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

    // If no joint names are included in the trajectory, then stop all followers
    if (trajectory.joint_names.size() == 0)
    {
        std::stringstream ss;
        ss << "The new joint trajectory contains no joint names, resetAll";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::WARN, ss.str());
        resetAll("new joint trajectory with no joint names");
    }
    else
    {
        actionlib_msgs::GoalID goalId;
        goalId.stamp = trajectory.header.stamp;
        goalId.id    = trajectory.header.frame_id;

        std::set<std::string> kasquishJoints;
        abort(goalId, kasquishJoints, "duplicate goal");
        preempt(trajectory.joint_names, kasquishJoints, "new joint breadcrumbs");
        // we aren't going to actually kasquish the joints in this case

        // Add the new goalId to the goalFollowerMap
        std::auto_ptr<PreplannedJointTrajectoryFollower> newFollower(new PreplannedJointTrajectoryFollower);
        newFollower->setTrajectory(trajectory, timeStep, velocityFactor);

        goalFollowerMap.insert(goalId, newFollower);
        sendStatusMessage(goalFollowerMap.at(goalId).goalManager.getStatus(), true);
        goalStatusMap.insert(std::make_pair(goalId, goalFollowerMap.at(goalId).goalManager.getStatus()));

        // Add the new joints to the jointGoalMap
        addToJointGoalMap(trajectory.joint_names, goalId);
    }
}

bool TrajectoryManager::isActive(const actionlib_msgs::GoalID& goalId) const
{
    return (goalFollowerMap.find(goalId) != goalFollowerMap.end());
}

void TrajectoryManager::abort(const actionlib_msgs::GoalID& goalId, const std::string& msg)
{
    std::set<std::string> kasquishJoints;
    abort(goalId, kasquishJoints, msg);
}

void TrajectoryManager::abort(const actionlib_msgs::GoalID& goalId, std::set<std::string>& abortedJoints, const std::string& msg)
{
    GoalFollowerMap_type::iterator followerIt = goalFollowerMap.find(goalId);

    if (followerIt != goalFollowerMap.end())
    {
        std::stringstream ss;
        ss << "aborting goal";

        if (!msg.empty())
        {
            ss << " (" << msg << ")";
        }

        ss << ": " << goalId;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::NOTICE, ss.str());

        const std::vector<std::string>& jointNames = followerIt->second->getJointNames();
        abortedJoints.insert(jointNames.begin(), jointNames.end());

        // abort
        followerIt->second->abort(msg);

        // send status update
        sendStatusMessage(followerIt->second->goalManager.getStatus(), true);

        // Remove the current goal from the goalFollowerIndexMap
        goalFollowerMap.erase(followerIt);

        // Remove the joints from the jointGoalMap
        removeFromJointGoalMap(goalId);
    }

    // remove from goal status map if in there
    goalStatusMap.erase(goalId);

    // remove from first point status map if in there
    goalReplanMap.erase(goalId);
}

void TrajectoryManager::kasquishAll(const sensor_msgs::JointState& startJointPositions,
                                    const sensor_msgs::JointState& startJointVels,
                                    const std::string& msg)
{
    std::stringstream ss;
    ss << "kasquishing all joints";
    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::NOTICE, ss.str());

    std::set<std::string> abortedJoints;

    for (GoalFollowerMap_type::iterator followerIt = goalFollowerMap.begin(); followerIt != goalFollowerMap.end(); ++followerIt)
    {
        const std::vector<std::string>& jointNames = followerIt->second->getJointNames();
        abortedJoints.insert(jointNames.begin(), jointNames.end());

        // abort
        followerIt->second->abort(msg);

        // add status update
        addStatus(followerIt->second->goalManager.getStatus());
    }

    // cleanup
    goalFollowerMap.clear();

    // remove from goal status map if in there
    goalStatusMap.clear();

    // remove from first point status map if in there
    goalReplanMap.clear();

    if (!abortedJoints.empty())
    {
        std::vector<std::string> kasquishVec(abortedJoints.size());
        std::copy(abortedJoints.begin(), abortedJoints.end(), kasquishVec.begin());
        kasquish(kasquishVec, startJointPositions, startJointVels);
    }
}

void TrajectoryManager::preempt(const std::vector<std::string>& jointNames, std::set<std::string>& preemptedJoints, const std::string& msg)
{
    // Ensure that no joints in the jointNames list are already stored in the jointGoalMap, if so preempt those trajectories
    for (std::vector<std::string>::const_iterator jointIt = jointNames.begin(); jointIt != jointNames.end(); ++jointIt)
    {
        std::map<std::string, actionlib_msgs::GoalID>::const_iterator conflictIt = jointGoalMap.find(*jointIt);

        if (conflictIt != jointGoalMap.end())
        {
            // get follower
            GoalFollowerMap_type::iterator followerIt = goalFollowerMap.find(conflictIt->second);

            if (followerIt != goalFollowerMap.end())
            {
                std::stringstream ss;
                ss << "joint conflict, preempting";
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::INFO, ss.str());

                const std::vector<std::string>& joints = followerIt->second->getJointNames();
                preemptedJoints.insert(joints.begin(), joints.end());

                // preempt
                followerIt->second->preempt(msg);

                // send status update
                sendStatusMessage(followerIt->second->goalManager.getStatus(), true);

                // Remove the current goal from the goalFollowerIndexMap
                goalFollowerMap.erase(followerIt);
            }

            // remove from goal status map if in there
            goalStatusMap.erase(conflictIt->second);

            // remove from first point status map if in there
            goalReplanMap.erase(conflictIt->second);

            // Remove the joint from the jointGoalMap
            removeFromJointGoalMap(conflictIt->second);
        }
    }
}

void TrajectoryManager::kasquish(const std::vector<std::string>& jointNames,
                                 const sensor_msgs::JointState& startJointPositions,
                                 const sensor_msgs::JointState& startJointVels)
{
    KDL::JntArray    startPose;
    KDL::JntArrayVel startVel;

    try
    {
        RosMsgConverter::JointStateToJntArray(startJointPositions, jointNames, startPose);
        RosMsgConverter::JointStateToJntArrayVel(startJointVels, jointNames, startVel);
    }
    catch (std::exception& e)
    {
        sendStatusMessage(ros::Time::now(), "kasquish", actionlib_msgs::GoalStatus::REJECTED, e.what(), true);
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, std::string("kasquish failed: ") + e.what());
        return;
    }

    rosJointTrajFactory.setLimits(jointNames, stopVelocityLimit, stopAccelerationLimit);

    for (unsigned int i = 0; i < jointNames.size(); ++i)
    {
        actionlib_msgs::GoalID goalId;
        goalId.id = jointNames[i] + " stop";

        trajectory_msgs::JointTrajectory trajectory;
        trajectory.header.frame_id = goalId.id;
        trajectory.joint_names.push_back(jointNames[i]);
        trajectory.points.resize(1);
        double dist = 0.5 * startVel.qdot(i) * startVel.qdot(i) / stopAccelerationLimit;

        if (startVel.qdot(i) < 0)
        {
            dist = -dist;
        }

        trajectory.points[0].positions.push_back(startPose(i) + dist);

        boost::shared_ptr<RosMsgJointTrajectory> onlineTraj;

        // get online trajectory
        try
        {
            onlineTraj = rosJointTrajFactory.getTrajectory(startJointPositions, startJointVels, startJointVels, trajectory);

            std::auto_ptr<OnlineJointTrajectoryFollower> newFollower(new OnlineJointTrajectoryFollower);
            newFollower->setTrajectory(onlineTraj, timeStep, velocityFactor);
            newFollower->goalManager.setStatus(actionlib_msgs::GoalStatus::ACTIVE);
            newFollower->goalManager.setText(jointNames[i] + " stopping");

            std::pair<GoalFollowerMap_type::iterator, bool> retVal = goalFollowerMap.insert(goalId, newFollower);
            const JointTrajectoryFollower* follower = retVal.first->second;
            addStatus(follower->goalManager.getStatus());
            goalStatusMap.insert(std::make_pair(goalId, follower->goalManager.getStatus()));

            // Add the new joints to the jointGoalMap
            addToJointGoalMap(onlineTraj->getJointNames(), goalId);
        }
        catch (std::exception& e)
        {
            // send message and stop
            addStatus(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, e.what());
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::ERROR, e.what());
        }
    }

    if (jointSettingsValid)
    {
        setJointSettings(jointSettings);
    }

    sendStatusMessage(true);
}

void TrajectoryManager::setupJointState(const r2_msgs::JointCommand& jointCommand)
{
    if (jointStateOut.name != jointCommand.name)
    {
        jointStateOut.name = jointCommand.name;
        jointIndexMap.clear();

        for (unsigned int i = 0; i < jointCommand.name.size(); ++i)
        {
            jointIndexMap[jointCommand.name[i]] = i;
        }

        jointStateOut.position.resize(jointCommand.name.size());
        jointStateOut.velocity.resize(jointCommand.name.size());
        jointStateOut.effort.resize(jointCommand.name.size());
    }

    for (unsigned int i = 0; i < jointCommand.name.size(); ++i)
    {
        jointStateOut.position[i] = jointCommand.desiredPosition[i];
        jointStateOut.velocity[i] = 0.;
        jointStateOut.effort[i]   = 0.;
    }
}

void TrajectoryManager::setupJointControl(const r2_msgs::JointControlDataArray& jointControl)
{
    for (unsigned int i = 0; i < jointControl.joint.size(); ++i)
    {
        if (jointControl.data[i].commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH
                || jointControl.data[i].commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP)
        {
            controllableJoints[jointControl.joint[i]] = true;
        }
        else
        {
            controllableJoints[jointControl.joint[i]] = false;
        }
    }
}

void TrajectoryManager::resetAll(const std::string& msg)
{
    std::stringstream ss;
    ss << "resetAll(): Stopping all followers...";
    NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryManager", log4cpp::Priority::NOTICE, ss.str());

    goalReplanMap.clear();

    if (goalFollowerMap.empty())
    {
        return;
    }

    for (GoalFollowerMap_type::iterator followerIt = goalFollowerMap.begin(); followerIt != goalFollowerMap.end(); ++followerIt)
    {
        followerIt->second->abort(msg);
        addStatus(followerIt->second->goalManager.getStatus());
    }

    goalFollowerMap.clear();

    sendStatusMessage(true);
}