JointTrajectoryFollower.cpp

Go to the documentation of this file.

#include <robodyn_controllers/JointTrajectoryFollower.h>

#include "nasa_common_logging/Logger.h"
/***************************************************************************/
JointTrajectoryFollower::JointTrajectoryFollower()
    : velocityFactor(1.)
{
    goalManager.setReadyState(false);
}
/***************************************************************************/
void JointTrajectoryFollower::abort(const std::string& msg)
{
    goalManager.setReadyState(false);
    goalManager.setStatus(actionlib_msgs::GoalStatus::ABORTED);
    goalManager.setText(msg);
}
/***************************************************************************/
void JointTrajectoryFollower::preempt(const std::string& msg)
{
    goalManager.setReadyState(false);
    goalManager.setStatus(actionlib_msgs::GoalStatus::PREEMPTED);
    goalManager.setText(msg);
}
/***************************************************************************/
PreplannedJointTrajectoryFollower::PreplannedJointTrajectoryFollower()
{
    trajIt = trajectory.points.end();
}
/***************************************************************************/
void PreplannedJointTrajectoryFollower::setTrajectory(const trajectory_msgs::JointTrajectory& trajectoryIn, double timestep_in, double velocityFactorIn)
{
    if (isActive())
    {
        //  active
        std::stringstream err;
        err << "PreplannedJointTrajectoryFollower::setTrajectory() - currently following a trajectory";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.PreplannedJointTrajectoryFollower", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    velocityFactor = velocityFactorIn;
    trajectory = trajectoryIn;
    trajIt = trajectory.points.begin();
    goalManager.reset(true);
    goalManager.setGoalId(trajectory.header.stamp, trajectory.header.frame_id);

    if (trajIt == trajectory.points.end())
    {
        goalManager.setReadyState(false);
        goalManager.setStatus(actionlib_msgs::GoalStatus::REJECTED);
        goalManager.setText("No points in trajectory.");
    }
    else if (trajIt->time_from_start != ros::Duration(0.0))
    {
        goalManager.setReadyState(false);
        goalManager.setStatus(actionlib_msgs::GoalStatus::REJECTED);
        goalManager.setText("Can't delay start.");
    }
    else
    {
        goalManager.setStatus(actionlib_msgs::GoalStatus::PENDING);
        goalManager.setText("Joint trajectory received at follower.");
    }


    currentTrajCount = 0;
    timestep = timestep_in;
}
/***************************************************************************/
void PreplannedJointTrajectoryFollower::getNextPoint(sensor_msgs::JointState& nextPoint, ros::Duration& timeFromStart)
{
    if (!isActive())
    {
        // not active
        std::stringstream err;
        err << "PreplannedJointTrajectoryFollower::getNextPoint() - trajectory is inactive";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.PreplannedJointTrajectoryFollower", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    if (trajIt == trajectory.points.end())
    {
        std::stringstream err;
        err << "PreplannedJointTrajectoryFollower::getNextPoint() - reached final trajectory point before allotted time";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.PreplannedJointTrajectoryFollower", log4cpp::Priority::INFO, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    nextPoint.header.frame_id = trajectory.header.frame_id;

    if (trajectory.joint_names.size() != trajIt->positions.size() || trajectory.joint_names.size() != trajIt->velocities.size()
            || trajectory.joint_names.size() != trajIt->accelerations.size())
    {
        std::stringstream err;
        err << "PreplannedJointTrajectoryFollower::getNextPoint() - trajectory names, positions, velocities, accelerations not properly sized";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.PreplannedJointTrajectoryFollower", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    if (currentTrajCount == 0)
    {
        nextPoint.name     = trajectory.joint_names;
        nextPoint.position = trajIt->positions;
        nextPoint.velocity.resize(trajIt->velocities.size());

        for (unsigned int i = 0; i < trajIt->velocities.size(); ++i)
        {
            nextPoint.velocity[i] = trajIt->velocities[i] * velocityFactor;
        }

        nextPoint.effort = trajIt->accelerations;
    }


    //If timestep is shorter than 30 hz:
    while ( trajIt->time_from_start <= ros::Duration(currentTrajCount * timestep))
    {
        nextPoint.name     = trajectory.joint_names;
        nextPoint.position = trajIt->positions;
        nextPoint.velocity.resize(trajIt->velocities.size());
        timeFromStart = trajIt->time_from_start;

        for (unsigned int i = 0; i < trajIt->velocities.size(); ++i)
        {
            nextPoint.velocity[i] = trajIt->velocities[i] * velocityFactor;
        }

        nextPoint.effort = trajIt->accelerations;
        trajIt++;

        if (trajIt == trajectory.points.end())
        {
            goalManager.setReadyState(false);
            goalManager.setStatus(actionlib_msgs::GoalStatus::SUCCEEDED);
            goalManager.setText("Joint trajectory finished.");
            return;
        }

        if (trajIt->time_from_start == ros::Duration(0.0))
        {
            goalManager.setStatus(actionlib_msgs::GoalStatus::ACTIVE);
            goalManager.setText("Joint trajectory sending next point.");
            return;
        }

        std::stringstream msg;
        msg << "PreplannedJointTrajectoryFollower::getNextPoint() - currentTrajCount: " << currentTrajCount << ", timestep: " << timestep << std::endl;
        msg << "PreplannedJointTrajectoryFollower::getNextPoint() - found a valid point. trajIt->time_from_start is " << trajIt->time_from_start << " and currentTrajCount*timestep is " << ros::Duration(currentTrajCount * timestep) ;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.PreplannedJointTrajectoryFollower", log4cpp::Priority::DEBUG, msg.str());

    }

    std::stringstream msg;
    msg << "PreplannedJointTrajectoryFollower::getNextPoint() - sending next point";
    NasaCommonLogging::Logger::log("gov.nasa.controllers.PreplannedJointTrajectoryFollower", log4cpp::Priority::INFO, msg.str());

    goalManager.setStatus(actionlib_msgs::GoalStatus::ACTIVE);
    goalManager.setText("Joint trajectory sending next point.");

    currentTrajCount++;

}
/***************************************************************************/
OnlineJointTrajectoryFollower::OnlineJointTrajectoryFollower()
    : time(0.), timestep(0.)
{
}
/***************************************************************************/
void OnlineJointTrajectoryFollower::setTrajectory(boost::shared_ptr<RosMsgJointTrajectory> trajectoryIn, double timestep_in, double velocityFactorIn)
{
    if (isActive())
    {
        //  active
        std::stringstream err;
        err << "OnlineJointTrajectoryFollower::setTrajectory() - currently following a trajectory";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.OnlineJointTrajectoryFollower", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    velocityFactor = velocityFactorIn;
    trajectory     = trajectoryIn;
    goalManager.reset(true);
    goalManager.setGoalId(trajectory->getGoalId());
    time     = 0.;
    timestep = timestep_in;
    goalManager.setStatus(actionlib_msgs::GoalStatus::PENDING);
    goalManager.setText("Joint trajectory received at follower.");
}
/***************************************************************************/
void OnlineJointTrajectoryFollower::getNextPoint(sensor_msgs::JointState& nextPoint, ros::Duration& timeFromStart)
{
    if (!isActive())
    {
        // not active
        std::stringstream err;
        err << "OnlineJointTrajectoryFollower::getNextPoint() - trajectory is inactive";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.OnlineJointTrajectoryFollower", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    bool deact = false;
    time += timestep;

    if (time >= trajectory->getDuration())
    {
        time = trajectory->getDuration();
        deact = true;
    }

    nextPoint.header.frame_id = trajectory->getGoalId().id;
    nextPoint.name            = trajectory->getJointNames();
    trajectory_msgs::JointTrajectoryPoint pose;
    trajectory->getPose(time, pose);
    nextPoint.position = pose.positions;
    nextPoint.velocity.resize(pose.velocities.size());

    for (unsigned int i = 0; i < pose.velocities.size(); ++i)
    {
        nextPoint.velocity[i] = pose.velocities[i] * velocityFactor;
    }

    nextPoint.effort = pose.accelerations;
    timeFromStart    = pose.time_from_start;

    if (deact)
    {
        goalManager.setReadyState(false);
        goalManager.setStatus(actionlib_msgs::GoalStatus::SUCCEEDED);
        goalManager.setText("Trajectory executed successfully!");
    }
    else
    {
        goalManager.setStatus(actionlib_msgs::GoalStatus::ACTIVE);
        goalManager.setText("Joint trajectory currently being executed.");
    }
}