From 23e230a834faaba6924900ac1737dffbc3ee86b4 Mon Sep 17 00:00:00 2001
From: rickstaa <rick.staa@outlook.com>
Date: Wed, 16 Feb 2022 12:43:07 +0100
Subject: [PATCH] feat(franka_gazebo): implement 'reset_joint_states' service

This commit implements a `reset_joint_states` service. This service can
be used to reset the join states when they are pushed outside of the
joint limits. This can for example happen when Gazebo's
`set_model_configuration` is used (see
https://github.com/frankaemika/franka_ros/issues/225 for more
information).
---
 .../include/franka_gazebo/franka_hw_sim.h          |  1 +
 franka_gazebo/include/franka_gazebo/joint.h        |  8 ++++++++
 franka_gazebo/src/franka_hw_sim.cpp                | 14 ++++++++++++++
 franka_gazebo/src/joint.cpp                        | 11 +++++++++++
 franka_msgs/CMakeLists.txt                         |  1 +
 franka_msgs/srv/ResetJointStates.srv               |  3 +++
 6 files changed, 38 insertions(+)
 create mode 100644 franka_msgs/srv/ResetJointStates.srv

diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
index 2cbd1fdd3..4e710526b 100644
--- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
+++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
@@ -115,6 +115,7 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   ros::ServiceServer service_set_k_;
   ros::ServiceServer service_set_load_;
   ros::ServiceServer service_collision_behavior_;
+  ros::ServiceServer service_reset_joint_states_;
 
   std::vector<double> lower_force_thresholds_nominal_;
   std::vector<double> upper_force_thresholds_nominal_;
diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h
index be8afcf3c..79898fa75 100644
--- a/franka_gazebo/include/franka_gazebo/joint.h
+++ b/franka_gazebo/include/franka_gazebo/joint.h
@@ -87,9 +87,17 @@ struct Joint {
    */
   bool isInCollision() const;
 
+  /**
+   * Makes sure the joint states are reset to zero.
+   */
+  void resetJointStates();
+
  private:
   double lastVelocity = std::numeric_limits<double>::quiet_NaN();
   double lastAcceleration = std::numeric_limits<double>::quiet_NaN();
+
+  // Used to track whether a reset of the joint states was requested
+  bool resetRequested_ = false;
 };
 
 }  // namespace franka_gazebo
diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
index d34d344fe..85e148b9e 100644
--- a/franka_gazebo/src/franka_hw_sim.cpp
+++ b/franka_gazebo/src/franka_hw_sim.cpp
@@ -5,11 +5,13 @@
 #include <franka_gazebo/model_kdl.h>
 #include <franka_hw/franka_hw.h>
 #include <franka_hw/services.h>
+#include <franka_msgs/ResetJointStates.h>
 #include <franka_msgs/SetEEFrame.h>
 #include <franka_msgs/SetForceTorqueCollisionBehavior.h>
 #include <franka_msgs/SetKFrame.h>
 #include <franka_msgs/SetLoad.h>
 #include <gazebo_ros_control/robot_hw_sim.h>
+#include <std_msgs/Empty.h>
 #include <Eigen/Dense>
 #include <iostream>
 #include <sstream>
@@ -305,6 +307,18 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
             response.success = true;
             return true;
           });
+  this->service_reset_joint_states_ = franka_hw::advertiseService<franka_msgs::ResetJointStates>(
+      nh, "reset_joint_states", [&](auto& request, auto& response) {
+        ROS_INFO_STREAM_NAMED("franka_hw_sim", "Resetting joint_states");
+
+        for (int i = 0; i < 7; i++) {
+          std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1);
+          this->joints_[name]->resetJointStates();
+        }
+
+        response.success = true;
+        return true;
+      });
 }
 
 void FrankaHWSim::readSim(ros::Time time, ros::Duration period) {
diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp
index d733539db..f2f456141 100644
--- a/franka_gazebo/src/joint.cpp
+++ b/franka_gazebo/src/joint.cpp
@@ -9,6 +9,12 @@ void Joint::update(const ros::Duration& dt) {
     return;
   }
 
+  // Reset joint states if requested
+  if (this->resetRequested_) {
+    this->position = 0;
+    this->resetRequested_ = false;
+  }
+
   this->velocity = this->handle->GetVelocity(0);
 #if GAZEBO_MAJOR_VERSION >= 8
   double position = this->handle->Position(0);
@@ -70,4 +76,9 @@ bool Joint::isInCollision() const {
 bool Joint::isInContact() const {
   return std::abs(this->effort - this->command) > this->contact_threshold;
 }
+
+void Joint::resetJointStates() {
+  this->resetRequested_ = true;
+}
+
 }  // namespace franka_gazebo
diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt
index 1f6f92659..a5a1c2ad1 100644
--- a/franka_msgs/CMakeLists.txt
+++ b/franka_msgs/CMakeLists.txt
@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_ms
 add_message_files(FILES Errors.msg FrankaState.msg)
 
 add_service_files(FILES
+  ResetJointStates.srv
   SetCartesianImpedance.srv
   SetEEFrame.srv
   SetForceTorqueCollisionBehavior.srv
diff --git a/franka_msgs/srv/ResetJointStates.srv b/franka_msgs/srv/ResetJointStates.srv
new file mode 100644
index 000000000..54b7e0cfd
--- /dev/null
+++ b/franka_msgs/srv/ResetJointStates.srv
@@ -0,0 +1,3 @@
+---
+bool success
+string error