TrajectoryMonitor.cpp

Go to the documentation of this file.

#include "robodyn_controllers/TrajectoryMonitor.h"

namespace SU = StringUtilities;

TrajectoryMonitor::TrajectoryMonitor(const std::string& name)
    : GoalStatusSender(name)
    , embeddedSuffix("embeddedCommand")
{
    defaultFactors.torqueLimitFactor = 2.5;
    defaultFactors.distanceFactor = 1.5;
    replanTraj.points.resize(1);
    replanTraj.points[0].time_from_start = ros::Duration(-1.);
}

TrajectoryMonitor::~TrajectoryMonitor()
{
}

void TrajectoryMonitor::setBases(const r2_msgs::StringArray& basesMsg)
{
    if (bases != basesMsg.data)
    {
        bases = basesMsg.data;
        writeBases(basesMsg);
    }
}

void TrajectoryMonitor::setFactors(const r2_msgs::TrajectoryMonitorFactors& factorsMsg)
{
    if (factorsMsg.joint_names.size() != factorsMsg.torqueLimitFactors.size()
            || factorsMsg.joint_names.size() != factorsMsg.distanceFactors.size())
    {
        std::stringstream ss;
        ss << "invalid factorsMsg was received";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
        return;
    }

    for (unsigned int i = 0; i < factorsMsg.joint_names.size(); ++i)
    {
        TrajectoryMonitorFactors& tempFactors = factors[factorsMsg.joint_names[i]];
        tempFactors.torqueLimitFactor = factorsMsg.torqueLimitFactors[i];
        tempFactors.distanceFactor = factorsMsg.distanceFactors[i];
    }
}

void TrajectoryMonitor::evaluateReplanTrigger(const sensor_msgs::JointState& jointStatesMsg, const r2_msgs::JointCommand& jointCommandMsg,
        const r2_msgs::JointControlDataArray& jointStatusMsg, const r2_msgs::JointCapability& jointCapMsg)
{
    JointData newJointData;
    jointData.clear();

    if (jointStatesMsg.name.size() != jointStatesMsg.position.size() || jointStatesMsg.name.size() != jointStatesMsg.velocity.size())
    {
        std::stringstream ss;
        ss << "invalid jointStatesMsg was received";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
        return;
    }

    if (jointCommandMsg.name.size() != jointCommandMsg.desiredPosition.size()
            || jointCommandMsg.name.size() != jointCommandMsg.desiredPositionVelocityLimit.size()
            || jointCommandMsg.name.size() != jointCommandMsg.proportionalGain.size()
            || jointCommandMsg.name.size() != jointCommandMsg.derivativeGain.size()
            || jointCommandMsg.name.size() != jointCommandMsg.positionLoopTorqueLimit.size())
    {
        std::stringstream ss;
        ss << "invalid jointCommandMsg was received";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
        return;
    }

    // add to map if in drive && torqueLim greater than 0
    for (unsigned int i = 0; i < jointStatusMsg.joint.size(); ++i)
    {
        if (jointStatusMsg.data.at(i).controlMode.state == r2_msgs::JointControlMode::DRIVE)
        {
            std::vector<std::string>::const_iterator capIt = std::find(jointCapMsg.name.begin(), jointCapMsg.name.end(), jointStatusMsg.joint[i]);

            if (capIt != jointCapMsg.name.end() && jointCapMsg.torqueLimit.at(capIt - jointCapMsg.name.begin()) > 0.)
            {
                jointData[jointStatusMsg.joint[i]] = newJointData;
            }
        }
    }

    // get position
    for (unsigned int i = 0; i < jointStatesMsg.name.size(); ++i)
    {
        std::string jnt = SU::getRoboDynJoint(jointStatesMsg.name[i]);

        if (jnt.find(embeddedSuffix) != std::string::npos && SU::isItemWithNumber(jnt))
        {
            // embedded
            jointDataIt = jointData.find(SU::getRoboDynEverythingButJoint(jointStatesMsg.name[i]) + SU::TOKEN_DELIMITER + "joint" + SU::getNumber(jnt));

            if (jointDataIt != jointData.end())
            {
                jointDataIt->second.embeddedCommand = true;
                jointDataIt->second.commandPosition = jointStatesMsg.position[i];
                jointDataIt->second.commandVelocity = jointStatesMsg.velocity[i];
            }

            continue;
        }

        // not embedded
        jointDataIt = jointData.find(jointStatesMsg.name[i]);

        if (jointDataIt != jointData.end())
        {
            jointDataIt->second.positionValid  = true;
            jointDataIt->second.actualPosition = jointStatesMsg.position[i];
            jointDataIt->second.actualVelocity = jointStatesMsg.velocity[i];
        }
    }

    // get command and replan
    for (unsigned int i = 0; i < jointCommandMsg.name.size(); ++i)
    {
        jointDataIt = jointData.find(jointCommandMsg.name[i]);

        if (jointDataIt != jointData.end() && jointDataIt->second.positionValid)
        {
            // get the factors
            double torqueLimitFactor = defaultFactors.torqueLimitFactor;
            double distanceFactor = defaultFactors.distanceFactor;
            std::map<std::string, TrajectoryMonitorFactors>::const_iterator factorIt = factors.find(jointCommandMsg.name[i]);

            if (factorIt != factors.end())
            {
                torqueLimitFactor = factorIt->second.torqueLimitFactor;
                distanceFactor = factorIt->second.distanceFactor;
            }

            jointDataIt->second.commandValid = true;

            // don't update if embedded was found
            if (!jointDataIt->second.embeddedCommand)
            {
                jointDataIt->second.commandPosition = jointCommandMsg.desiredPosition[i];
                jointDataIt->second.commandVelocity = jointCommandMsg.desiredPositionVelocityLimit[i];
            }

            double posTorque = jointCommandMsg.proportionalGain[i] * (jointDataIt->second.commandPosition - jointDataIt->second.actualPosition);

            if (posTorque > torqueLimitFactor * jointCommandMsg.positionLoopTorqueLimit[i])
            {
                jointDataIt->second.replan.type = r2_msgs::ReplanType::PAUSE;
                jointDataIt->second.torqueLimitCommand = jointDataIt->second.actualPosition +
                        distanceFactor * (jointCommandMsg.positionLoopTorqueLimit[i] / jointCommandMsg.proportionalGain[i]);
                std::stringstream ss;
                ss << "replan for " << jointDataIt->first;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::NOTICE, ss.str());
                ss << ":";
                ss << "\n\tcommandPosition: " << jointDataIt->second.commandPosition;
                ss << "\n\tactualPosition:  " << jointDataIt->second.actualPosition;
                ss << "\n\tactualVelocity:  " << jointDataIt->second.actualVelocity;
                ss << "\n\tposTorque:       " << posTorque;
                ss << "\n\ttorqueLimit:     " << torqueLimitFactor* jointCommandMsg.positionLoopTorqueLimit[i];
                ss << "\n\tnewCommand:      " << jointDataIt->second.torqueLimitCommand;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
            }
            else if (posTorque < -torqueLimitFactor * jointCommandMsg.positionLoopTorqueLimit[i])
            {
                jointDataIt->second.replan.type = r2_msgs::ReplanType::PAUSE;
                jointDataIt->second.torqueLimitCommand = jointDataIt->second.actualPosition -
                        distanceFactor * (jointCommandMsg.positionLoopTorqueLimit[i] / jointCommandMsg.proportionalGain[i]);
                std::stringstream ss;
                ss << "replan for " << jointDataIt->first;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::NOTICE, ss.str());
                ss << ":";
                ss << "\n\tcommandPosition: " << jointDataIt->second.commandPosition;
                ss << "\n\tactualPosition:  " << jointDataIt->second.actualPosition;
                ss << "\n\tactualVelocity:  " << jointDataIt->second.actualVelocity;
                ss << "\n\tposTorque:       " << posTorque;
                ss << "\n\ttorqueLimit:     " << torqueLimitFactor* jointCommandMsg.positionLoopTorqueLimit[i];
                ss << "\n\tnewCommand:      " << jointDataIt->second.torqueLimitCommand;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
            }
        }
    }
}

void TrajectoryMonitor::replan()
{

    replanTraj.joint_names.clear();
    replanTraj.points[0].positions.clear();
    replanTraj.points[0].velocities.clear();
    replanVec.clear();

    std::stringstream replanJoints;

    for (jointDataIt = jointData.begin(); jointDataIt != jointData.end(); ++jointDataIt)
    {
        if (jointDataIt->second.replan.type == r2_msgs::ReplanType::PAUSE && jointDataIt->second.commandValid)
        {
            replanTraj.joint_names.push_back(jointDataIt->first);
            replanVec.push_back(jointDataIt->second.replan);
            replanTraj.points[0].positions.push_back(jointDataIt->second.torqueLimitCommand);
            replanTraj.points[0].velocities.push_back(jointDataIt->second.actualVelocity);

            replanJoints << jointDataIt->first << ", ";
        }
    }

    if (!replanTraj.joint_names.empty())
    {
        replanTraj.header.stamp = ros::Time::now();
        replanTraj.header.frame_id = std::string("joints ") + replanJoints.str() + " replan:hold";

        addJointWaypoints(replanTraj, replanVec);
    }

}

void TrajectoryMonitor::updateMonitor(const r2_msgs::JointCommand& jointCommandMsg,
                                      const r2_msgs::PoseState& poseCommandsMsg)
{
    if (jointCommandMsg.name.size() != jointCommandMsg.desiredPosition.size())
    {
        std::stringstream ss;
        ss << "invalid jointCommandMsg was received";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
        return;
    }

    // put joint positions in map
    std::map<std::string, double> jointCommands;

    for (unsigned int i = 0; i < jointCommandMsg.name.size(); ++i)
    {
        jointCommands[jointCommandMsg.name[i]] = jointCommandMsg.desiredPosition[i];
    }

    std::map<std::string, double>::const_iterator jointIt;

    // check joint goal achievement
    for (JointTrajectoriesMap_type::iterator trajIt = jointTrajectories.begin(); trajIt != jointTrajectories.end();)
    {
        bool achieved = true;

        for (unsigned int i = 0; i < trajIt->second.joint_names.size(); ++i)
        {
            jointIt = jointCommands.find(trajIt->second.joint_names[i]);

            if (jointIt == jointCommands.end())
            {
                std::stringstream ss;
                ss << "no position available for trajectory (" << trajIt->first.stamp.toSec() << ") " << trajIt->first.id << " joint "
                   << trajIt->second.joint_names[i] << ". Removing trajectory from monitored list.";
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
                JointTrajectoriesMap_type::iterator tempIt = trajIt++;
                jointTrajectories.erase(tempIt);
                achieved = false;
                break;
            }
            else
            {
                // check positions size on receipt so this is safe
                if (fabs(trajIt->second.points[0].positions[i] - jointIt->second) > 0.0001)
                {
                    achieved = false;
                    ++trajIt;
                    break;
                }
            }
        }

        if (achieved)
        {
            std::stringstream ss;
            ss << "waypoint reached";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
            sendStatusMessage(trajIt->first.stamp, trajIt->first.id, actionlib_msgs::GoalStatus::ACTIVE, ss.str(), true);

            if (trajIt->second.points.size() == 1)
            {
                JointTrajectoriesMap_type::iterator tempIt = trajIt++;
                jointTrajectories.erase(tempIt);
            }
            else
            {
                // this is inefficient!!! However, we shouldn't do it very often and the vectors are likely small.
                trajIt->second.points.erase(trajIt->second.points.begin());
                // don't index trajIt because we want to check the next point
            }
        }
    }

    // check pose goal achievement
    for (PoseTrajectoriesMap_type::iterator trajIt = poseTrajectories.begin(); trajIt != poseTrajectories.end();)
    {
        bool achieved = true;

        for (unsigned int nodeIndex = 0; nodeIndex < trajIt->second.nodes.size(); ++nodeIndex)
        {
            try
            {
                KDL::Frame actualFrame, goalFrame;
                RosMsgConverter::PoseStateToFrame(poseCommandsMsg, trajIt->second.refFrames[nodeIndex], trajIt->second.nodes[nodeIndex], actualFrame);
                tf::poseMsgToKDL(trajIt->second.points[0].positions[nodeIndex], goalFrame);
                KDL::Twist twist = KDL::diff(goalFrame, actualFrame);

                for (unsigned int axisIndex = 0; axisIndex < 3; ++axisIndex)
                {
                    if ((fabs(twist.vel[axisIndex]) > 0.0001 && trajIt->second.node_priorities[nodeIndex].axis_priorities[axisIndex] == r2_msgs::PriorityArray::CRITICAL)
                            || (fabs(twist.rot[axisIndex]) > 0.0001 && trajIt->second.node_priorities[nodeIndex].axis_priorities[3 + axisIndex] == r2_msgs::PriorityArray::CRITICAL))
                    {
                        achieved = false;
                        break;
                    }
                }
            }
            catch (std::exception& e)
            {
                std::stringstream ss;
                ss << "invalid jointCommandMsg was received -- " << e.what();
                NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
                return;
            }
        }

        if (achieved)
        {
            std::stringstream ss;
            ss << "waypoint reached";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
            sendStatusMessage(trajIt->first.stamp, trajIt->first.id, actionlib_msgs::GoalStatus::ACTIVE, ss.str(), true);

            if (trajIt->second.points.size() == 1)
            {
                PoseTrajectoriesMap_type::iterator tempIt = trajIt++;
                poseTrajectories.erase(tempIt);
            }
            else
            {
                // this is inefficient!!! However, we shouldn't do it very often and the vectors are likely small.
                trajIt->second.points.erase(trajIt->second.points.begin());
                // don't index trajIt because we want to check the next point
            }
        }
        else
        {
            ++trajIt;
        }
    }
}

void TrajectoryMonitor::updateGoals(const actionlib_msgs::GoalStatusArray& goalStatuses)
{
    writeStatus(goalStatuses);

    // handle completion
    for (unsigned int i = 0; i < goalStatuses.status_list.size(); ++i)
    {
        if (goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::SUCCEEDED
                || goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::ABORTED
                || goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::PREEMPTED
                || goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::REJECTED)
        {
            jointTrajectories.erase(goalStatuses.status_list[i].goal_id);
            poseTrajectories.erase(goalStatuses.status_list[i].goal_id);
        }
    }
}

void TrajectoryMonitor::addCartesianWaypoints(r2_msgs::PoseTrajectory trajectory, const r2_msgs::PoseState& poseStatesMsg)
{
    // if empty, just pass along
    if (trajectory.nodes.size() == 0 || trajectory.points.size() == 0)
    {
        sendCartesianWaypoints(trajectory);
        return;
    }

    // verify trajectory input and massage the trajectory for this purpose
    if (bases.size() == 0)
    {
        std::stringstream ss;
        ss << "no bases set";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        return;
    }

    if (trajectory.refFrames.size() == 1)
    {
        trajectory.refFrames.resize(trajectory.nodes.size(), trajectory.refFrames[0]);
    }
    else if (trajectory.refFrames.size() != trajectory.nodes.size())
    {
        std::stringstream ss;
        ss << "invalid refFrames";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        return;
    }

    if (trajectory.node_priorities.size() == 0)
    {
        trajectory.node_priorities.resize(trajectory.nodes.size());
    }
    else if (trajectory.node_priorities.size() != trajectory.nodes.size())
    {
        std::stringstream ss;
        ss << "invalid node_priorities";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
        return;
    }

    for (unsigned int i = 0; i < trajectory.node_priorities.size(); ++i)
    {
        if (trajectory.node_priorities[i].axis_priorities.size() == 0)
        {
            trajectory.node_priorities[i].axis_priorities.resize(6, r2_msgs::PriorityArray::CRITICAL);
        }
        else if (trajectory.node_priorities[i].axis_priorities.size() == 1)
        {
            trajectory.node_priorities[i].axis_priorities.resize(6, trajectory.node_priorities[i].axis_priorities[0]);
        }
        else if (trajectory.node_priorities[i].axis_priorities.size() != 6)
        {
            std::stringstream ss;
            ss << "invalid node_priorities";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
            sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
            return;
        }
    }

    bool valid = true;

    for (unsigned int pointIndex = 0; pointIndex < trajectory.points.size(); ++pointIndex)
    {
        if (trajectory.points[pointIndex].positions.size() != trajectory.nodes.size())
        {
            valid = false;
            break;
        }
        else
        {
            for (unsigned int posIndex = 0; posIndex < trajectory.points[pointIndex].positions.size(); ++posIndex)
            {
                KDL::FrameAcc refFrame, goalFrame;

                try
                {
                    RosMsgConverter::PoseStateToFrameAcc(poseStatesMsg, bases[0], trajectory.refFrames[posIndex], refFrame);
                }
                catch (std::exception& e)
                {
                    valid = false;
                    break;
                }

                // get goal
                goalFrame.p.p.x(trajectory.points[pointIndex].positions[posIndex].position.x);
                goalFrame.p.p.y(trajectory.points[pointIndex].positions[posIndex].position.y);
                goalFrame.p.p.z(trajectory.points[pointIndex].positions[posIndex].position.z);
                goalFrame.M.R = KDL::Rotation::Quaternion(trajectory.points[pointIndex].positions[posIndex].orientation.x,
                                trajectory.points[pointIndex].positions[posIndex].orientation.y,
                                trajectory.points[pointIndex].positions[posIndex].orientation.z,
                                trajectory.points[pointIndex].positions[posIndex].orientation.w);

                // velocity
                if (trajectory.points[pointIndex].velocities.size() == trajectory.nodes.size())
                {
                    goalFrame.p.v.x(trajectory.points[pointIndex].velocities[posIndex].linear.x);
                    goalFrame.p.v.y(trajectory.points[pointIndex].velocities[posIndex].linear.y);
                    goalFrame.p.v.z(trajectory.points[pointIndex].velocities[posIndex].linear.z);
                    goalFrame.M.w.x(trajectory.points[pointIndex].velocities[posIndex].angular.x);
                    goalFrame.M.w.y(trajectory.points[pointIndex].velocities[posIndex].angular.y);
                    goalFrame.M.w.z(trajectory.points[pointIndex].velocities[posIndex].angular.z);
                }
                else if (trajectory.points[pointIndex].velocities.size() != 0)
                {
                    valid = false;
                    break;
                }

                // acceleration
                if (trajectory.points[pointIndex].accelerations.size() == trajectory.nodes.size())
                {
                    goalFrame.p.dv.x(trajectory.points[pointIndex].accelerations[posIndex].linear.x);
                    goalFrame.p.dv.y(trajectory.points[pointIndex].accelerations[posIndex].linear.y);
                    goalFrame.p.dv.z(trajectory.points[pointIndex].accelerations[posIndex].linear.z);
                    goalFrame.M.dw.x(trajectory.points[pointIndex].accelerations[posIndex].angular.x);
                    goalFrame.M.dw.y(trajectory.points[pointIndex].accelerations[posIndex].angular.y);
                    goalFrame.M.dw.z(trajectory.points[pointIndex].accelerations[posIndex].angular.z);
                }
                else if (trajectory.points[pointIndex].accelerations.size() != 0)
                {
                    valid = false;
                    break;
                }

                // convert to new refFrame
                goalFrame = refFrame * goalFrame;

                // put back in proper place
                trajectory.points[pointIndex].positions[posIndex].position.x = goalFrame.p.p.x();
                trajectory.points[pointIndex].positions[posIndex].position.y = goalFrame.p.p.y();
                trajectory.points[pointIndex].positions[posIndex].position.z = goalFrame.p.p.z();
                goalFrame.M.R.GetQuaternion(trajectory.points[pointIndex].positions[posIndex].orientation.x,
                                            trajectory.points[pointIndex].positions[posIndex].orientation.y,
                                            trajectory.points[pointIndex].positions[posIndex].orientation.z,
                                            trajectory.points[pointIndex].positions[posIndex].orientation.w);

                // velocity
                if (trajectory.points[pointIndex].velocities.size() == trajectory.nodes.size())
                {
                    trajectory.points[pointIndex].velocities[posIndex].linear.x = goalFrame.p.v.x();
                    trajectory.points[pointIndex].velocities[posIndex].linear.y = goalFrame.p.v.y();
                    trajectory.points[pointIndex].velocities[posIndex].linear.z = goalFrame.p.v.z();
                    trajectory.points[pointIndex].velocities[posIndex].angular.x = goalFrame.M.w.x();
                    trajectory.points[pointIndex].velocities[posIndex].angular.y = goalFrame.M.w.y();
                    trajectory.points[pointIndex].velocities[posIndex].angular.z = goalFrame.M.w.z();
                }

                // acceleration
                if (trajectory.points[pointIndex].accelerations.size() == trajectory.nodes.size())
                {
                    trajectory.points[pointIndex].accelerations[posIndex].linear.x = goalFrame.p.dv.x();
                    trajectory.points[pointIndex].accelerations[posIndex].linear.y = goalFrame.p.dv.y();
                    trajectory.points[pointIndex].accelerations[posIndex].linear.z = goalFrame.p.dv.z();
                    trajectory.points[pointIndex].accelerations[posIndex].angular.x = goalFrame.M.dw.x();
                    trajectory.points[pointIndex].accelerations[posIndex].angular.y = goalFrame.M.dw.y();
                    trajectory.points[pointIndex].accelerations[posIndex].angular.z = goalFrame.M.dw.z();
                }
            }
        }
    }

    if (valid)
    {
        // reset refFrames
        trajectory.refFrames = std::vector<std::string>(trajectory.nodes.size(), bases[0]);
        actionlib_msgs::GoalID goalId;
        goalId.stamp = trajectory.header.stamp;
        goalId.id = trajectory.header.frame_id;
        poseTrajectories[goalId] = trajectory;
        sendCartesianWaypoints(trajectory);
    }
    else
    {
        std::stringstream ss;
        ss << "invalid trajectory";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
    }
}

void TrajectoryMonitor::addJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
        const std::vector<r2_msgs::ReplanType>& replan)
{
    // if empty, just pass along
    if (trajectory.joint_names.size() == 0 || trajectory.points.size() == 0)
    {
        sendJointWaypoints(trajectory, replan);
        return;
    }

    // verify trajectory input for this purpose
    bool valid = true;

    for (unsigned int i = 0; i < trajectory.points.size(); ++i)
    {
        if (trajectory.points[i].positions.size() != trajectory.joint_names.size())
        {
            valid = false;
            break;
        }
    }

    if (valid)
    {
        actionlib_msgs::GoalID goalId;
        goalId.stamp              = trajectory.header.stamp;
        goalId.id                 = trajectory.header.frame_id;
        jointTrajectories[goalId] = trajectory;
        sendJointWaypoints(trajectory, replan);
    }
    else
    {
        std::stringstream ss;
        ss << "invalid trajectory";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
        sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
    }
}

void TrajectoryMonitor::sendCartesianWaypoints(const r2_msgs::PoseTrajectory& trajectory,
        const r2_msgs::ReplanType::_type_type& replan)
{
    r2_msgs::PoseTrajectoryReplan traj;
    traj.header.stamp = ros::Time::now();
    traj.replan.type  = replan;
    traj.trajectory   = trajectory;
    writeCartesianWaypoints(traj);
}

void TrajectoryMonitor::sendJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
        const std::vector<r2_msgs::ReplanType>& replan)
{
    r2_msgs::JointTrajectoryReplan traj;
    traj.header.stamp = ros::Time::now();
    traj.replan       = replan;
    traj.trajectory   = trajectory;
    writeJointWaypoints(traj);
}