From 1bc061ec7207c8dd33b5eda7e5a914f8f58ae495 Mon Sep 17 00:00:00 2001
From: Francisco Vina <fevb@kth.se>
Date: Fri, 12 Dec 2014 18:05:30 +0100
Subject: [PATCH] adding bias calib service in gravity compensation

Conflicts:
	gravity_compensation/CMakeLists.txt
---
 gravity_compensation/CMakeLists.txt           |  4 +-
 gravity_compensation/package.xml              |  2 +
 .../src/gravity_compensation_node.cpp         | 46 +++++++++++++++++++
 3 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/gravity_compensation/CMakeLists.txt b/gravity_compensation/CMakeLists.txt
index e3ab1c5..29fa175 100644
--- a/gravity_compensation/CMakeLists.txt
+++ b/gravity_compensation/CMakeLists.txt
@@ -2,13 +2,13 @@ cmake_minimum_required(VERSION 2.8.3)
 project(gravity_compensation)
 
 
-find_package(catkin REQUIRED COMPONENTS cmake_modules roscpp geometry_msgs sensor_msgs tf tf_conversions eigen_conversions)
+find_package(catkin REQUIRED COMPONENTS cmake_modules roscpp geometry_msgs sensor_msgs tf tf_conversions eigen_conversions std_srvs)
 
 find_package(Boost REQUIRED COMPONENTS thread)
 
 catkin_package(
   DEPENDS eigen
-  CATKIN_DEPENDS geometry_msgs sensor_msgs tf tf_conversions eigen_conversions
+  CATKIN_DEPENDS geometry_msgs sensor_msgs tf tf_conversions eigen_conversions std_srvs
   INCLUDE_DIRS include 
   LIBRARIES gravity_compensation
 )
diff --git a/gravity_compensation/package.xml b/gravity_compensation/package.xml
index ab0fc11..73d401e 100644
--- a/gravity_compensation/package.xml
+++ b/gravity_compensation/package.xml
@@ -20,6 +20,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>tf_conversions</build_depend>
   <build_depend>eigen_conversions</build_depend>
+  <build_depend>std_srvs</build_depend>
 
   <run_depend>cmake_modules</run_depend>
   <run_depend>roscpp</run_depend>
@@ -29,5 +30,6 @@
   <run_depend>tf_conversions</run_depend>
   <run_depend>eigen</run_depend>
   <run_depend>eigen_conversions</run_depend>
+  <run_depend>std_srvs</run_depend>
 
 </package>
diff --git a/gravity_compensation/src/gravity_compensation_node.cpp b/gravity_compensation/src/gravity_compensation_node.cpp
index 1707621..2337acf 100644
--- a/gravity_compensation/src/gravity_compensation_node.cpp
+++ b/gravity_compensation/src/gravity_compensation_node.cpp
@@ -43,6 +43,7 @@
 #include <tf/transform_broadcaster.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <boost/thread.hpp>
+#include <std_srvs/Empty.h>
 
 
 class GravityCompensationNode
@@ -57,6 +58,8 @@ class GravityCompensationNode
 	ros::Publisher topicPub_ft_zeroed_;
 	ros::Publisher topicPub_ft_compensated_;
 
+    ros::ServiceServer calibrate_bias_srv_server_;
+
 	tf::TransformBroadcaster tf_br_;
 
 
@@ -66,11 +69,17 @@ class GravityCompensationNode
 		m_g_comp_params  = new GravityCompensationParams();
 		m_g_comp = NULL;
 		m_received_imu = false;
+        m_calibrate_bias = false;
+        m_calib_measurements = 0;
+        m_ft_bias = Eigen::Matrix<double, 6, 1>::Zero();
 
 		// subscribe to accelerometer topic and raw F/T sensor topic
 		topicSub_imu_ = n_.subscribe("imu", 1, &GravityCompensationNode::topicCallback_imu, this);
 		topicSub_ft_raw_ = n_.subscribe("ft_raw", 1, &GravityCompensationNode::topicCallback_ft_raw, this);
 
+        // bias calibration service
+        calibrate_bias_srv_server_ = n_.advertiseService("calibrate_bias", &GravityCompensationNode::calibrateBiasSrvCallback, this);
+
 		/// implementation of topics to publish
 		std::string ns;
 		if(n_.hasParam("ns"))
@@ -243,6 +252,30 @@ class GravityCompensationNode
 	{
 		static int error_msg_count=0;
 
+        if(m_calibrate_bias)
+        {
+            if(m_calib_measurements++<100)
+            {
+                m_ft_bias(0) += msg->wrench.force.x;
+                m_ft_bias(1) += msg->wrench.force.y;
+                m_ft_bias(2) += msg->wrench.force.z;
+                m_ft_bias(3) += msg->wrench.torque.x;
+                m_ft_bias(4) += msg->wrench.torque.y;
+                m_ft_bias(5) += msg->wrench.torque.z;
+            }
+
+            // set the new bias
+            if(m_calib_measurements == 100)
+            {
+                m_ft_bias = m_ft_bias/100;
+                m_g_comp_params->setBias(m_g_comp_params->getBias() + m_ft_bias);
+                m_ft_bias = Eigen::Matrix<double, 6, 1>::Zero();
+                m_calibrate_bias = false;
+                m_calib_measurements = 0;
+            }
+
+        }
+
 		if(!m_received_imu)
 		{
 			return;
@@ -287,6 +320,15 @@ class GravityCompensationNode
 		}
 	}
 
+    // only to be called when the robot is standing still and
+    // while not holding anything / applying any forces
+    bool calibrateBiasSrvCallback(std_srvs::Empty::Request &req,
+                                  std_srvs::Empty::Response &res)
+    {
+        m_calibrate_bias = true;
+        return true;
+    }
+
 private:
 
 	GravityCompensationParams *m_g_comp_params;
@@ -295,6 +337,10 @@ class GravityCompensationNode
 	bool m_received_imu;
 	double m_gripper_com_broadcast_frequency;
 
+    bool m_calibrate_bias;
+    unsigned int m_calib_measurements;
+    Eigen::Matrix<double, 6, 1> m_ft_bias;
+
 };
 
 int main(int argc, char **argv)