joint_with_effort_trajectory_controller_impl.h

Go to the documentation of this file.

//
// Created by tom on 18/09/16.
//

#ifndef ROBOTICAN_CONTROLLERS_JOINT_WITH_EFFORT_TRAJECTORY_CONTROLLER_IMPL_H
#define ROBOTICAN_CONTROLLERS_JOINT_WITH_EFFORT_TRAJECTORY_CONTROLLER_IMPL_H

namespace joint_with_effort_trajectory_controller
{

    namespace internal
    {

        template <class Enclosure, class Member>
        inline boost::shared_ptr<Member> share_member(boost::shared_ptr<Enclosure> enclosure, Member &member)
        {
            actionlib::EnclosureDeleter<Enclosure> d(enclosure);
            boost::shared_ptr<Member> p(&member, d);
            return p;
        }

        std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
        {
            using namespace XmlRpc;
            XmlRpcValue xml_array;
            if (!nh.getParam(param_name, xml_array))
            {
                ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
                return std::vector<std::string>();
            }
            if (xml_array.getType() != XmlRpcValue::TypeArray)
            {
                ROS_ERROR_STREAM("The '" << param_name << "' parameter is not an array (namespace: " <<
                                         nh.getNamespace() << ").");
                return std::vector<std::string>();
            }

            std::vector<std::string> out;
            for (int i = 0; i < xml_array.size(); ++i)
            {
                if (xml_array[i].getType() != XmlRpcValue::TypeString)
                {
                    ROS_ERROR_STREAM("The '" << param_name << "' parameter contains a non-string element (namespace: " <<
                                             nh.getNamespace() << ").");
                    return std::vector<std::string>();
                }
                out.push_back(static_cast<std::string>(xml_array[i]));
            }
            return out;
        }

        boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
        {
            boost::shared_ptr<urdf::Model> urdf(new urdf::Model);

            std::string urdf_str;
            // Check for robot_description in proper namespace
            if (nh.getParam(param_name, urdf_str))
            {
                if (!urdf->initString(urdf_str))
                {
                    ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
                                                                           nh.getNamespace() << ").");
                    return boost::shared_ptr<urdf::Model>();
                }
            }
                // Check for robot_description in root
            else if (!urdf->initParam("robot_description"))
            {
                ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
                return boost::shared_ptr<urdf::Model>();
            }
            return urdf;
        }

        typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
        std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
        {
            std::vector<UrdfJointConstPtr> out;
            for (unsigned int i = 0; i < joint_names.size(); ++i)
            {
                UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
                if (urdf_joint)
                {
                    out.push_back(urdf_joint);
                }
                else
                {
                    ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
                    return std::vector<UrdfJointConstPtr>();
                }
            }
            return out;
        }

        std::string getLeafNamespace(const ros::NodeHandle& nh)
        {
            const std::string complete_ns = nh.getNamespace();
            std::size_t id   = complete_ns.find_last_of("/");
            return complete_ns.substr(id + 1);
        }

    } // namespace

    template <class SegmentImpl, class HardwareInterface>
    inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    starting(const ros::Time& time)
    {
        // Update time data
        TimeData time_data;
        time_data.time   = time;
        time_data.uptime = ros::Time(0.0);
        time_data_.initRT(time_data);

        // Hold current position
        setHoldPosition(time_data.uptime);

        // Initialize last state update time
        last_state_publish_time_ = time_data.uptime;

        // Hardware interface adapter
        hw_iface_adapter_.starting(time_data.uptime);
    }

    template <class SegmentImpl, class HardwareInterface>
    inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    stopping(const ros::Time& /*time*/)
    {
        preemptActiveGoal();
    }

    template <class SegmentImpl, class HardwareInterface>
    inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    trajectoryCommandCB(const JointTrajectoryConstPtr& msg)
    {
        const bool update_ok = updateTrajectoryCommand(msg, RealtimeGoalHandlePtr());
        if (update_ok) {preemptActiveGoal();}
    }

    template <class SegmentImpl, class HardwareInterface>
    inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    preemptActiveGoal()
    {
        RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);

        // Cancels the currently active goal
        if (current_active_goal)
        {
            // Marks the current goal as canceled
            rt_active_goal_.reset();
            current_active_goal->gh_.setCanceled();
        }
    }

    template <class SegmentImpl, class HardwareInterface>
    inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    checkPathTolerances(const typename Segment::State& state_error,
                        const Segment&                 segment)
    {
        const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
        const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
        ROS_INFO("CUR: %f, Goal: %f", _currentEffort, _goalEffort);
        if(_goalEffort != 0 && _currentEffort > _goalEffort) {
            ROS_INFO("MAX effort");
            rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
            rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_);
            rt_active_goal_.reset();
        }
        else if (!checkStateTolerance(state_error, tolerances.state_tolerance))
        {
            rt_segment_goal->preallocated_result_->error_code =
                    control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
            rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
            rt_active_goal_.reset();
        }
    }

    template <class SegmentImpl, class HardwareInterface>
    inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    checkGoalTolerances(const typename Segment::State& state_error,
                        const Segment&                 segment)
    {
        // Controller uptime
        const ros::Time uptime = time_data_.readFromRT()->uptime;

        // Checks that we have ended inside the goal tolerances
        const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
        const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
        const bool inside_goal_tolerances = checkStateTolerance(state_error, tolerances.goal_state_tolerance);

        if (inside_goal_tolerances || (_goalEffort != 0 && _currentEffort >= _goalEffort))
        {
            rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
            rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_);
            rt_active_goal_.reset();
            if(_goalEffort != 0 && _currentEffort >= _goalEffort) {
                ROS_INFO("MAX effort");
            }
        }
        else if (uptime.toSec() < segment.endTime() + tolerances.goal_time_tolerance)
        {
            // Still have some time left to meet the goal state tolerances
        }
        else
        {
            if (verbose_)
            {
                ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed");
                // Check the tolerances one more time to output the errors that occures
                checkStateTolerance(state_error, tolerances.goal_state_tolerance, true);
            }

            rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
            rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
            rt_active_goal_.reset();
        }
    }

    template <class SegmentImpl, class HardwareInterface>
    JointTrajectoryController<SegmentImpl, HardwareInterface>::
    JointTrajectoryController()
            : verbose_(false), // Set to true during debugging
              hold_trajectory_ptr_(new Trajectory)
    {}

    template <class SegmentImpl, class HardwareInterface>
    bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInterface* hw,
                                                                         ros::NodeHandle&   root_nh,
                                                                         ros::NodeHandle&   controller_nh)
    {
        using namespace internal;
        // Cache controller node handle
        controller_nh_ = controller_nh;

        // Controller name
        name_ = getLeafNamespace(controller_nh_);

        // State publish rate
        double state_publish_rate = 50.0;
        controller_nh_.getParam("state_publish_rate", state_publish_rate);
        ROS_DEBUG_STREAM_NAMED(name_, "Controller state will be published at " << state_publish_rate << "Hz.");
        state_publisher_period_ = ros::Duration(1.0 / state_publish_rate);

        // Action status checking update rate
        double action_monitor_rate = 20.0;
        controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
        action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
        ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");

        // Stop trajectory duration
        stop_trajectory_duration_ = 0.0;
        _currentEffort = 0;
        if (!controller_nh_.getParam("stop_trajectory_duration", stop_trajectory_duration_))
        {
            // TODO: Remove this check/warning in Indigo
            if (controller_nh_.getParam("hold_trajectory_duration", stop_trajectory_duration_))
            {
                ROS_WARN("The 'hold_trajectory_duration' has been deprecated in favor of the 'stop_trajectory_duration' parameter. Please update your controller configuration.");
            }
        }
        ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s.");

        // List of controlled joints
        joint_names_ = getStrings(controller_nh_, "joints");
        if (joint_names_.empty()) {return false;}
        const unsigned int n_joints = joint_names_.size();

        // URDF joints
        boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
        if (!urdf) {return false;}

        std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
        if (urdf_joints.empty()) {return false;}
        assert(n_joints == urdf_joints.size());

        // Initialize members
        joints_.resize(n_joints);
        angle_wraparound_.resize(n_joints);
        for (unsigned int i = 0; i < n_joints; ++i)
        {
            // Joint handle
            try {joints_[i] = hw->getHandle(joint_names_[i]);}
            catch (...)
            {
                ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_names_[i] << "' in '" <<
                                                                       this->getHardwareInterfaceType() << "'.");
                return false;
            }

            // Whether a joint is continuous (ie. has angle wraparound)
            angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS;
            const std::string not_if = angle_wraparound_[i] ? "" : "non-";

            ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" <<
                                                   this->getHardwareInterfaceType() << "'.");
        }

        assert(joints_.size() == angle_wraparound_.size());
        ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
                                                                 "\n- Number of joints: " << joints_.size() <<
                                                                 "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" <<
                                                                 "\n- Trajectory segment type: '" << hardware_interface::internal::demangledTypeName<SegmentImpl>() << "'");

        // Default tolerances
        ros::NodeHandle tol_nh(controller_nh_, "constraints");
        default_tolerances_ = getSegmentTolerances<Scalar>(tol_nh, joint_names_);

        // Hardware interface adapter
        hw_iface_adapter_.init(joints_, controller_nh_);

        // ROS API: Subscribed topics
        trajectory_command_sub_ = controller_nh_.subscribe("command", 1, &JointTrajectoryController::trajectoryCommandCB, this);

        // ROS API: Published topics
        state_publisher_.reset(new StatePublisher(controller_nh_, "state", 1));

        // ROS API: Action interface
        action_server_.reset(new ActionServer(controller_nh_, "follow_joint_trajectory",
                                              boost::bind(&JointTrajectoryController::goalCB,   this, _1),
                                              boost::bind(&JointTrajectoryController::cancelCB, this, _1),
                                              false));
        action_server_->start();

        // ROS API: Provided services
        query_state_service_ = controller_nh_.advertiseService("query_state",
                                                               &JointTrajectoryController::queryStateService,
                                                               this);

        // Preeallocate resources
        current_state_    = typename Segment::State(n_joints);
        desired_state_    = typename Segment::State(n_joints);
        state_error_      = typename Segment::State(n_joints);
        hold_start_state_ = typename Segment::State(n_joints);
        hold_end_state_   = typename Segment::State(n_joints);

        Segment hold_segment(0.0, current_state_, 0.0, current_state_);
        hold_trajectory_ptr_->resize(1, hold_segment);

        {
            state_publisher_->lock();
            state_publisher_->msg_.joint_names = joint_names_;
            state_publisher_->msg_.desired.positions.resize(n_joints);
            state_publisher_->msg_.desired.velocities.resize(n_joints);
            state_publisher_->msg_.desired.accelerations.resize(n_joints);
            state_publisher_->msg_.actual.positions.resize(n_joints);
            state_publisher_->msg_.actual.velocities.resize(n_joints);
            state_publisher_->msg_.error.positions.resize(n_joints);
            state_publisher_->msg_.error.velocities.resize(n_joints);
            state_publisher_->unlock();
        }

        return true;
    }

    template <class SegmentImpl, class HardwareInterface>
    void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    update(const ros::Time& time, const ros::Duration& period)
    {
        // Get currently followed trajectory
        TrajectoryPtr curr_traj_ptr;
        curr_trajectory_box_.get(curr_traj_ptr);
        Trajectory& curr_traj = *curr_traj_ptr;

        // Update time data
        TimeData time_data;
        time_data.time   = time;                                     // Cache current time
        time_data.period = period;                                   // Cache current control period
        time_data.uptime = time_data_.readFromRT()->uptime + period; // Update controller uptime
        time_data_.writeFromNonRT(time_data); // TODO: Grrr, we need a lock-free data structure here!

        // NOTE: It is very important to execute the two above code blocks in the specified sequence: first get current
        // trajectory, then update time data. Hopefully the following paragraph sheds a bit of light on the rationale.
        // The non-rt thread responsible for processing new commands enqueues trajectories that can start at the _next_
        // control cycle (eg. zero start time) or later (eg. when we explicitly request a start time in the future).
        // If we reverse the order of the two blocks above, and update the time data first; it's possible that by the time we
        // fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts in the
        // next control cycle, leaving the current cycle without a valid trajectory.

        // Update desired state: sample trajectory at current time
        typename Trajectory::const_iterator segment_it = sample(curr_traj, time_data.uptime.toSec(), desired_state_);
        if (curr_traj.end() == segment_it)
        {
            // Non-realtime safe, but should never happen under normal operation
            ROS_ERROR_NAMED(name_,
                            "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
            return;
        }

        // Update current state and state error
        double maxCurrentEffort = -100.0;
        for (unsigned int i = 0; i < joints_.size(); ++i)
        {
            current_state_.position[i] = joints_[i].getPosition();
            current_state_.velocity[i] = joints_[i].getVelocity();
            maxCurrentEffort = (maxCurrentEffort <= joints_[i].getEffort()) ? joints_[i].getEffort() : maxCurrentEffort;
            // There's no acceleration data available in a joint handle

            state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];
            state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i];
            state_error_.acceleration[i] = 0.0;
        }
        _currentEffort = maxCurrentEffort;
        // Check tolerances if segment corresponds to currently active action goal
        const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
        if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
        {
            // Check tolerances
            if (time_data.uptime.toSec() < segment_it->endTime())
            {
                // Currently executing a segment: check path tolerances
                checkPathTolerances(state_error_,
                                    *segment_it);
            }
            else if (segment_it == --curr_traj.end())
            {
                if (verbose_)
                    ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segement, checking goal tolerances");

                // Finished executing the LAST segment: check goal tolerances
                checkGoalTolerances(state_error_,
                                    *segment_it);
            }
        }

        // Hardware interface adapter: Generate and send commands
        hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,
                                        desired_state_, state_error_);

        // Publish state
        publishState(time_data.uptime);
    }

    template <class SegmentImpl, class HardwareInterface>
    bool JointTrajectoryController<SegmentImpl, HardwareInterface>::
    updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh)
    {
        typedef InitJointTrajectoryOptions<Trajectory> Options;

        // Preconditions
        if (!this->isRunning())
        {
            ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
            return false;
        }

        if (!msg)
        {
            ROS_WARN_NAMED(name_, "Received null-pointer trajectory message, skipping.");
            return false;
        }

        // Time data
        TimeData* time_data = time_data_.readFromRT(); // TODO: Grrr, we need a lock-free data structure here!

        // Time of the next update
        const ros::Time next_update_time = time_data->time + time_data->period;

        // Uptime of the next update
        ros::Time next_update_uptime = time_data->uptime + time_data->period;

        // Hold current position if trajectory is empty
        if (msg->points.empty())
        {
            setHoldPosition(time_data->uptime);
            ROS_DEBUG_NAMED(name_, "Empty trajectory command, stopping.");
            return true;
        }

        // Trajectory initialization options
        TrajectoryPtr curr_traj_ptr;
        curr_trajectory_box_.get(curr_traj_ptr);

        Options options;
        options.other_time_base    = &next_update_uptime;
        options.current_trajectory = curr_traj_ptr.get();
        options.joint_names        = &joint_names_;
        options.angle_wraparound   = &angle_wraparound_;
        options.rt_goal_handle     = gh;
        options.default_tolerances = &default_tolerances_;

        // Update currently executing trajectory
        try
        {
            TrajectoryPtr traj_ptr(new Trajectory);
            *traj_ptr = initJointTrajectory<Trajectory>(*msg, next_update_time, options);
            if (!traj_ptr->empty())
            {
                curr_trajectory_box_.set(traj_ptr);
            }
            else
            {
                // All trajectory points are in the past, nothing new to execute. Keep on executing current trajectory
                return false;
            }
        }
        catch(const std::invalid_argument& ex)
        {
            ROS_ERROR_STREAM_NAMED(name_, ex.what());
            return false;
        }
        catch(...)
        {
            ROS_ERROR_NAMED(name_, "Unexpected exception caught when initializing trajectory from ROS message data.");
            return false;
        }

        return true;
    }

    template <class SegmentImpl, class HardwareInterface>
    void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    goalCB(GoalHandle gh)
    {
        ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");

        // Precondition: Running controller
        if (!this->isRunning())
        {
            ROS_ERROR_NAMED(name_, "Can't accept new action goals. Controller is not running.");
            control_msgs::FollowJointTrajectoryResult result;
            result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; // TODO: Add better error status to msg?
            gh.setRejected(result);
            return;
        }

        // Goal should specify all controller joints (they can be ordered differently). Reject if this is not the case
        using joint_trajectory_controller::internal::permutation;
        std::vector<unsigned int> permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names);

        if (permutation_vector.empty())
        {
            ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints.");
            control_msgs::FollowJointTrajectoryResult result;
            result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
            gh.setRejected(result);
            return;
        }

        // Try to update new trajectory
        RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
        const bool update_ok = updateTrajectoryCommand(internal::share_member(gh.getGoal(), gh.getGoal()->trajectory),
                                                       rt_goal);

        if (update_ok)
        {
            // Accept new goal
            preemptActiveGoal();
            gh.setAccepted();
            rt_active_goal_ = rt_goal;
            size_t  size = rt_goal->gh_.getGoal()->trajectory.points.size();
            if(!rt_goal->gh_.getGoal()->trajectory.points.empty() && !rt_goal->gh_.getGoal()->trajectory.points[size - 1].effort.empty()) {

                _goalEffort = rt_goal->gh_.getGoal()->trajectory.points[size - 1].effort[0];
                ROS_INFO("goal effort is: %f", _goalEffort);
            } else {
                _goalEffort = 0.0;
            }
            // Setup goal status checking timer
            goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
                                                            &RealtimeGoalHandle::runNonRealtime,
                                                            rt_goal);
            goal_handle_timer_.start();
        }
        else
        {
            // Reject invalid goal
            control_msgs::FollowJointTrajectoryResult result;
            result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
            gh.setRejected(result);
        }
    }

    template <class SegmentImpl, class HardwareInterface>
    void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    cancelCB(GoalHandle gh)
    {
        RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);

        // Check that cancel request refers to currently active goal (if any)
        if (current_active_goal && current_active_goal->gh_ == gh)
        {
            // Reset current goal
            rt_active_goal_.reset();

            // Controller uptime
            const ros::Time uptime = time_data_.readFromRT()->uptime;

            // Enter hold current position mode
            setHoldPosition(uptime);
            ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");

            // Mark the current goal as canceled
            current_active_goal->gh_.setCanceled();
        }
    }

    template <class SegmentImpl, class HardwareInterface>
    bool JointTrajectoryController<SegmentImpl, HardwareInterface>::
    queryStateService(control_msgs::QueryTrajectoryState::Request&  req,
                      control_msgs::QueryTrajectoryState::Response& resp)
    {
        // Preconditions
        if (!this->isRunning())
        {
            ROS_ERROR_NAMED(name_, "Can't sample trajectory. Controller is not running.");
            return false;
        }

        // Convert request time to internal monotonic representation
        TimeData* time_data = time_data_.readFromRT();
        const ros::Duration time_offset = req.time - time_data->time;
        const ros::Time sample_time = time_data->uptime + time_offset;

        // Sample trajectory at requested time
        TrajectoryPtr curr_traj_ptr;
        curr_trajectory_box_.get(curr_traj_ptr);
        Trajectory& curr_traj = *curr_traj_ptr;

        typename Segment::State state;
        typename Trajectory::const_iterator segment_it = sample(curr_traj, sample_time.toSec(), state);
        if (curr_traj.end() == segment_it)
        {
            ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time.");
            return false;
        }

        // Populate response
        resp.name         = joint_names_;
        resp.position     = state.position;
        resp.velocity     = state.velocity;
        resp.acceleration = state.acceleration;

        return true;
    }

    template <class SegmentImpl, class HardwareInterface>
    void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    publishState(const ros::Time& time)
    {
        // Check if it's time to publish
        if (!state_publisher_period_.isZero() && last_state_publish_time_ + state_publisher_period_ < time)
        {
            if (state_publisher_ && state_publisher_->trylock())
            {
                last_state_publish_time_ += state_publisher_period_;

                state_publisher_->msg_.header.stamp          = time_data_.readFromRT()->time;
                state_publisher_->msg_.desired.positions     = desired_state_.position;
                state_publisher_->msg_.desired.velocities    = desired_state_.velocity;
                state_publisher_->msg_.desired.accelerations = desired_state_.acceleration;
                state_publisher_->msg_.actual.positions      = current_state_.position;
                state_publisher_->msg_.actual.velocities     = current_state_.velocity;
                state_publisher_->msg_.error.positions       = state_error_.position;
                state_publisher_->msg_.error.velocities      = state_error_.velocity;

                state_publisher_->unlockAndPublish();
            }
        }
    }

    template <class SegmentImpl, class HardwareInterface>
    void JointTrajectoryController<SegmentImpl, HardwareInterface>::
    setHoldPosition(const ros::Time& time)
    {
        // Settle position in a fixed time. We do the following:
        // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time
        // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity
        // - Create segment that goes from current state to above zero velocity state, in the desired time
        // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types

        assert(1 == hold_trajectory_ptr_->size());

        const typename Segment::Time start_time  = time.toSec();
        const typename Segment::Time end_time    = time.toSec() + stop_trajectory_duration_;
        const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_;

        // Create segment that goes from current (pos,vel) to (pos,-vel)
        const unsigned int n_joints = joints_.size();
        for (unsigned int i = 0; i < n_joints; ++i)
        {
            hold_start_state_.position[i]     =  joints_[i].getPosition();
            hold_start_state_.velocity[i]     =  joints_[i].getVelocity();
            hold_start_state_.acceleration[i] =  0.0;

            hold_end_state_.position[i]       =  joints_[i].getPosition();
            hold_end_state_.velocity[i]       = -joints_[i].getVelocity();
            hold_end_state_.acceleration[i]   =  0.0;
        }
        hold_trajectory_ptr_->front().init(start_time,  hold_start_state_,
                                           end_time_2x, hold_end_state_);

        // Sample segment at its midpoint, that should have zero velocity
        hold_trajectory_ptr_->front().sample(end_time, hold_end_state_);

        // Now create segment that goes from current state to one with zero end velocity
        hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
                                           end_time,   hold_end_state_);

        curr_trajectory_box_.set(hold_trajectory_ptr_);
    }

} // namespace

#endif //ROBOTICAN_CONTROLLERS_JOINT_WITH_EFFORT_TRAJECTORY_CONTROLLER_IMPL_H