Merge pull request #198 in SWDEV/franka_ros from fix/gazebo-service-n…

…amespaces to develop * commit 'c424884a7d51224ab3f297dfacae22c7b9ac959a': FIX: Make change BREAKING FIX: Increase stop speed tolerance in franka_gazebo test CHANGE: Move franka_gazebo service namespaces into `franka_control`
frankaemika · Aug 4, 2022 · 54ab726 · 54ab726
2 parents c929aa1 + c424884
commit 54ab726
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 11 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -18,6 +18,7 @@
   * `franka_gazebo`: Properly calculate inertial properties of `world/stone/model.sdf`
   * `franka_gazebo`: `set_user_stop` service to simulate User stop in Gazebo
   * `franka_gazebo`: `error_recovery` action similar to `franka_control`
+  * **BREAKING**: `franka_gazebo`: Move services like `set_EE_frame`, `set_K_frame` ... into `franka_control` namespace to be more consistent with real robot
 
 ## 0.9.0 - 2022-03-29
 

diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
@@ -332,7 +332,7 @@ void FrankaHWSim::initFrankaModelHandle(
 void FrankaHWSim::initServices(ros::NodeHandle& nh) {
   this->service_set_ee_ =
       nh.advertiseService<franka_msgs::SetEEFrame::Request, franka_msgs::SetEEFrame::Response>(
-          "set_EE_frame", [&](auto& request, auto& response) {
+          "franka_control/set_EE_frame", [&](auto& request, auto& response) {
             ROS_INFO_STREAM_NAMED("franka_hw_sim",
                                   this->arm_id_ << ": Setting NE_T_EE transformation");
             std::copy(request.NE_T_EE.cbegin(), request.NE_T_EE.cend(),
@@ -342,7 +342,7 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
             return true;
           });
   this->service_set_k_ = franka_hw::advertiseService<franka_msgs::SetKFrame>(
-      nh, "set_K_frame", [&](auto& request, auto& response) {
+      nh, "franka_control/set_K_frame", [&](auto& request, auto& response) {
         ROS_INFO_STREAM_NAMED("franka_hw_sim", this->arm_id_ << ": Setting EE_T_K transformation");
         std::copy(request.EE_T_K.cbegin(), request.EE_T_K.cend(),
                   this->robot_state_.EE_T_K.begin());
@@ -351,7 +351,7 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
         return true;
       });
   this->service_set_load_ = franka_hw::advertiseService<franka_msgs::SetLoad>(
-      nh, "set_load", [&](auto& request, auto& response) {
+      nh, "franka_control/set_load", [&](auto& request, auto& response) {
         ROS_INFO_STREAM_NAMED("franka_hw_sim", this->arm_id_ << ": Setting Load");
         this->robot_state_.m_load = request.mass;
         std::copy(request.F_x_center_load.cbegin(), request.F_x_center_load.cend(),
@@ -364,7 +364,8 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
       });
   this->service_collision_behavior_ =
       franka_hw::advertiseService<franka_msgs::SetForceTorqueCollisionBehavior>(
-          nh, "set_force_torque_collision_behavior", [&](auto& request, auto& response) {
+          nh, "franka_control/set_force_torque_collision_behavior",
+          [&](auto& request, auto& response) {
             ROS_INFO_STREAM_NAMED("franka_hw_sim", this->arm_id_ << ": Setting Collision Behavior");
 
             for (int i = 0; i < 7; i++) {
@@ -387,7 +388,7 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
           });
   this->service_user_stop_ =
       nh.advertiseService<std_srvs::SetBool::Request, std_srvs::SetBool::Response>(
-          "set_user_stop", [&](auto& request, auto& response) {
+          "franka_control/set_user_stop", [&](auto& request, auto& response) {
             this->sm_.process_event(UserStop{static_cast<bool>(request.data)});
             response.success = true;
             return true;

diff --git a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp
@@ -23,7 +23,8 @@ using franka::RobotMode;
 namespace sml = boost::sml;
 
 // NOTE: Keep a global node handle in memory in order for all tests to be executed sequentially
-// If each test or fixture declares its own node handle only the first test will be executed correctly
+// If each test or fixture declares its own node handle only the first test will be executed
+// correctly
 std::unique_ptr<ros::NodeHandle> nh;
 
 TEST(
@@ -302,7 +303,7 @@ struct FrankaHWSimFixture : public ::testing::Test {
 
   virtual void SetUp() {
     start_ = ros::Time::now().toSec();
-    user_stop = nh->serviceClient<std_srvs::SetBool>("set_user_stop");
+    user_stop = nh->serviceClient<std_srvs::SetBool>("/franka_control/set_user_stop");
     gripper = std::make_unique<actionlib::SimpleActionClient<franka_gripper::MoveAction>>(
         "/franka_gripper/move");
     error_recovery =
@@ -324,9 +325,9 @@ struct FrankaHWSimFixture : public ::testing::Test {
     franka_gripper::MoveGoal goal;
     goal.speed = 0.1;
     goal.width = 0.0;
-      gripper->sendGoalAndWait(goal, ros::Duration(5));
-      auto result = gripper->getResult();
-      ASSERT_TRUE(result->success) << "Failed to close the gripper, error: " << result->error;
+    gripper->sendGoalAndWait(goal, ros::Duration(5));
+    auto result = gripper->getResult();
+    ASSERT_TRUE(result->success) << "Failed to close the gripper, error: " << result->error;
   }
 
   void errorRecovery() {
@@ -360,7 +361,7 @@ TEST_F(FrankaHWSimFixture, pressing_user_stop_stops_the_arm_joints) {
   double user_stop_time = 3;
   double assert_start_time = 3.5;
   double assert_end_time = 4.5;
-  double stop_speed_tolerance = 1e-2;  // [rad/s]
+  double stop_speed_tolerance = 15e-3;  // [rad/s]
 
   ros::Duration timeout(5);