joint_trajectory_controller.h

Go to the documentation of this file.

//
// Created by tom on 10/04/16.
//

#ifndef ROBOTICAN_CONTROLLERS_JOINT_TRAJECTORY_CONTROLLE_H
#define ROBOTICAN_CONTROLLERS_JOINT_TRAJECTORY_CONTROLLE_H

#include <cassert>
#include <iterator>
#include <stdexcept>
#include <string>

// Boost
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>

// ROS
#include <ros/node_handle.h>

// URDF
#include <urdf/model.h>

// ROS messages
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <control_msgs/JointTrajectoryControllerState.h>
#include <control_msgs/QueryTrajectoryState.h>
#include <trajectory_msgs/JointTrajectory.h>

// actionlib
#include <actionlib/server/action_server.h>

// realtime_tools
#include <realtime_tools/realtime_box.h>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>

// ros_controls
#include <realtime_tools/realtime_server_goal_handle.h>
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/internal/demangle_symbol.h>

// Project
#include <trajectory_interface/trajectory_interface.h>

#include <joint_trajectory_controller/joint_trajectory_segment.h>
#include <joint_trajectory_controller/init_joint_trajectory.h>
#include <robotican_controllers/hardware_interface_adapter.h>

namespace joint_trajectory_controller
{

    template <class SegmentImpl, class HardwareInterface>
    class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>
    {
    public:

        JointTrajectoryController();

        bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
        /*\}*/

        void starting(const ros::Time& time);

        void stopping(const ros::Time& /*time*/);

        void update(const ros::Time& time, const ros::Duration& period);
        /*\}*/

    private:

        struct TimeData
        {
            TimeData() : time(0.0), period(0.0), uptime(0.0) {}

            ros::Time     time;   
            ros::Duration period; 
            ros::Time     uptime; 
        };

        typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>                  ActionServer;
        typedef boost::shared_ptr<ActionServer>                                                     ActionServerPtr;
        typedef ActionServer::GoalHandle                                                            GoalHandle;
        typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
        typedef boost::shared_ptr<RealtimeGoalHandle>                                               RealtimeGoalHandlePtr;
        typedef trajectory_msgs::JointTrajectory::ConstPtr                                          JointTrajectoryConstPtr;
        typedef realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState>     StatePublisher;
        typedef boost::scoped_ptr<StatePublisher>                                                   StatePublisherPtr;

        typedef JointTrajectorySegment<SegmentImpl> Segment;
        typedef std::vector<Segment> Trajectory;
        typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
        typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
        typedef typename Segment::Scalar Scalar;

        typedef HardwareInterfaceAdapter<HardwareInterface, typename Segment::State> HwIfaceAdapter;
        typedef typename HardwareInterface::ResourceHandleType JointHandle;

        bool                      verbose_;            
        std::string               name_;               
        std::vector<JointHandle>  joints_;             
        std::vector<bool>         angle_wraparound_;   
        std::vector<std::string>  joint_names_;        
        SegmentTolerances<Scalar> default_tolerances_; 
        HwIfaceAdapter            hw_iface_adapter_;   

        RealtimeGoalHandlePtr     rt_active_goal_;     

        TrajectoryBox curr_trajectory_box_;
        TrajectoryPtr hold_trajectory_ptr_; 

        typename Segment::State current_state_;    
        typename Segment::State desired_state_;    
        typename Segment::State state_error_;      
        typename Segment::State hold_start_state_; 
        typename Segment::State hold_end_state_;   

        realtime_tools::RealtimeBuffer<TimeData> time_data_;

        ros::Duration state_publisher_period_;
        ros::Duration action_monitor_period_;

        typename Segment::Time stop_trajectory_duration_;

        // ROS API
        ros::NodeHandle    controller_nh_;
        ros::Subscriber    trajectory_command_sub_;
        ActionServerPtr    action_server_;
        ros::ServiceServer query_state_service_;
        StatePublisherPtr  state_publisher_;
        bool               _isAborted;

        ros::Timer         goal_handle_timer_;
        ros::Time          last_state_publish_time_;

        bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh);
        void trajectoryCommandCB(const JointTrajectoryConstPtr& msg);
        void goalCB(GoalHandle gh);
        void cancelCB(GoalHandle gh);
        void preemptActiveGoal();
        bool queryStateService(control_msgs::QueryTrajectoryState::Request&  req,
                               control_msgs::QueryTrajectoryState::Response& resp);

        void publishState(const ros::Time& time);

        void setHoldPosition(const ros::Time& time);

        void checkPathTolerances(const typename Segment::State& state_error,
                                 const Segment&                 segment);

        void checkGoalTolerances(const typename Segment::State& state_error,
                                 const Segment&                 segment);

    };

} // namespace

#include <robotican_controllers/joint_trajectory_controller_impl.h>

#endif //ROBOTICAN_CONTROLLERS_JOINT_TRAJECTORY_CONTROLLE_H