text: // Copyright (C) 2020  Gokhan Solak, CRISP, Queen Mary University of London
  // This code is licensed under the BSD 3-Clause license (see LICENSE for details)
  
  
  #ifndef SPRING_SERVER_H
  #define SPRING_SERVER_H
  
  #include <functional>
  #include <memory>
  
  #include <ros/ros.h>
  #include <ros/spinner.h>
  #include <ros/callback_queue.h>
  #include <tf/transform_broadcaster.h>
  
  #include <spring_framework/spring_display.h>
  #include <spring_framework/spring_network.h>
  #include <spring_framework/spring_control.h>
  
  #include <kdl_conversions/kdl_msg.h>
  #include <eigen_conversions/eigen_kdl.h>
  
  #include <geometry_msgs/PoseArray.h>
  #include <kdl_control_tools/WrenchArray.h>
  #include <geometry_msgs/WrenchStamped.h>
  #include <geometry_msgs/TransformStamped.h>
  
  #include <kdl_control_tools/kdl_helper.h>
  #include <kdl_control_tools/progress_logger.h>
  
  using namespace std;
  
  namespace spring_framework
  {
  
  // SpringServer object runs a ros node to control a spring system.
  // It handles the communication to read the state and publish
  // cartesian forces generated by springs.
  // It also publishes spring displays for RViz.
  // This class is independent from robot hand hardware.
  class SpringServer
  {
    private:
  
      // object control params
      double k_rot_;
      double k_pos_;
      double k_vel_;
  
      double control_freq_;
  
      // object control targets
      KDL::Frame pose_des_; // desired object frame
      KDL::Vector vel_des_; // desired object velocity
      bool control_object_;
  
      // spring count
      // contact point frames (typically fingertip frames)
      // are stored as the first elements of frame_vec_.
      int contact_count_;
  
      // virtual spring objects are stored in the network
      shared_ptr<SpringNetwork> network_;
      shared_ptr<SpringController> control_;
      shared_ptr<SpringDisplay> display_;
  
      // communication
      ros::Publisher grasp_force_pub_; // to control the robot
      ros::Publisher manipulation_force_pub_; // to control the robot
      ros::Subscriber pose_sub_; // to track the state
      ros::Timer control_timer_; // to update the control
      vector<ros::Publisher> wrench_pub_vec_; // to visualize in rviz
  
      ros::Publisher spring_pub_; // publish the last state of the spring network
      bool publish_springs_;
  
      tf::TransformBroadcaster tf_pub_;
      bool publish_transform_;
  
      // node related
      bool active_; // controlled with start() stop() methods
      bool ready_; // set true when the first frames are received
      shared_ptr<ros::NodeHandle> nh_parent_;
  
      // handler functions
      function<void(const SpringServer*, const vector<KDL::Frame>&)> *handle_update_enter_;
      function<void(const SpringServer*, const vector<KDL::Frame>&)> *handle_update_exit_;
  
      // timing
      ros::Time t_update_;
  
      // debugging
      shared_ptr<kdl_control_tools::ProgressLogger> p_logger_;
  
      void publishCartForces_(const vector<geometry_msgs::Wrench> &wrench_vec);
      void framePoseCallback_(const geometry_msgs::PoseArray::ConstPtr &msg);
      void visualizeFingerForce_(const int finger_no, const geometry_msgs::Vector3& force_vec, const KDL::Frame& ff_frame);
      void publishObjectTransform_(const KDL::Frame& vf, const string& tf_name, const string& parent_tf_name);
      void timerCallback_(const ros::TimerEvent&);
  
      // procedure that updates and publishes the forces
      virtual void updateForces_(const vector<KDL::Frame>& frame_vec);
  
    public:
  
      SpringServer(int contact_count, shared_ptr<SpringNetwork>network, bool publish_springs=true, bool publish_transform=true, double control_frequency=200);
      ~SpringServer();
  
      // Activates the control
      // requires a NodeHandle of the parent node
      void start(shared_ptr<ros::NodeHandle> nh_parent);
      // Deactivates the control
      void stop();
  
      // this method manually updates the frames used in spring network
      virtual void setFrames(const vector<KDL::Frame>& frame_vec);
  
      // Handlers to allow user to do something during the loop
      // it takes a SpringServer* (that calls it) and the latest frames.
      void setUpdateEnterHandler(function<void(const SpringServer*, const vector<KDL::Frame>&)> &fp);
      void setUpdateExitHandler(function<void(const SpringServer*, const vector<KDL::Frame>&)> &fp);
  
      // starts/stops spring publishing
      void togglePublishSprings(const bool activate);
  
      // starts/stops transform broadcasting for the object virtual frame
      void togglePublishTransform(const bool activate);
  
      // set & get controller targets
      // setting a desired pose also activates object-level impedance control
      // pass a nullptr to deactivate
      void setDesiredPose(KDL::Frame p_des);
      void setDesiredVelocity(KDL::Vector v_des);
  
      const KDL::Frame getDesiredPose() const;
      const KDL::Vector getDesiredVelocity() const;
  
      // activate/deactivate object control
      void setObjectControl(bool state);
      bool getObjectControl();
  
      // other getters and setters
        // getters
        // FIXME: return reference instead of a pointer
      const shared_ptr<SpringNetwork> getNetwork() const;
      const shared_ptr<SpringController> getController() const;
      double getPositionGain() const;
      double getVelocityGain() const;
      double getRotationGain() const;
        // setters
      void setPositionGain(const double k);
      void setVelocityGain(const double k);
      void setRotationGain(const double k);
  
      // debugging
      void connectLogger(shared_ptr<kdl_control_tools::ProgressLogger> logger);
  
  };
  
  } // end namespace spring_framework
  
  #endif // SPRING_SERVER_H
  