wrench_manager.hpp

Go to the documentation of this file.

#ifndef __WRENCH_MANAGER__
#define __WRENCH_MANAGER__

#include <eigen_conversions/eigen_kdl.h>
#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/WrenchStamped.h>
#include <kdl_conversions/kdl_msg.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <Eigen/Dense>
#include <generic_control_toolbox/manager_base.hpp>
#include <generic_control_toolbox/matrix_parser.hpp>

namespace generic_control_toolbox
{
class WrenchManager : public ManagerBase
{
 public:
  WrenchManager(ros::NodeHandle nh_ = ros::NodeHandle("~"));
  ~WrenchManager();

  bool initializeWrenchComm(const std::string &end_effector,
                            const std::string &sensor_frame,
                            const std::string &gripping_point_frame,
                            const std::string &sensor_topic,
                            const std::string &calib_matrix_param);

  bool setGrippingPoint(const std::string &end_effector,
                        const std::string &gripping_point_frame);

  bool wrenchAtGrippingPoint(const std::string &end_effector,
                             Eigen::Matrix<double, 6, 1> &wrench) const;

  bool wrenchAtSensorPoint(const std::string &end_effector,
                           Eigen::Matrix<double, 6, 1> &wrench) const;

  bool wrenchAtSensorPointInGraspFrame(const std::string &end_effector, 
                                       Eigen::Matrix<double, 6, 1> &wrench) const;

 private:
  int max_tf_attempts_;
  std::map<std::string, std::string> sensor_frame_;
  std::map<std::string, KDL::Frame> sensor_to_gripping_point_;
  std::map<std::string, KDL::Wrench> measured_wrench_;
  std::map<std::string, ros::Subscriber> ft_sub_;
  std::map<std::string, ros::Publisher> processed_ft_pub_;
  std::map<std::string, std::string> gripping_frame_;
  std::map<std::string, Eigen::Matrix<double, 6, 6> > calibration_matrix_;
  tf::TransformListener listener_;
  MatrixParser parser_;
  ros::NodeHandle nh_;

  void forceTorqueCB(const geometry_msgs::WrenchStamped::ConstPtr &msg);
};

bool setWrenchManager(const ArmInfo &arm_info, WrenchManager &manager);
bool setWrenchManager(const ArmInfo &arm_info,
                      std::shared_ptr<WrenchManager> &manager);
}  // namespace generic_control_toolbox
#endif