franka_hw::FrankaCombinableHW V1

frankaemika · Jul 9, 2019 · 89a4f4f · 89a4f4f
1 parent af0a806
commit 89a4f4f
Show file tree

Hide file tree

Showing 30 changed files with 813 additions and 238 deletions.
diff --git a/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h b/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h
@@ -24,14 +24,14 @@
 #include <ros/time.h>
 #include <urdf/model.h>
 
-#include <franka_control/ErrorRecoveryAction.h>
-#include <franka_control/services.h>
 #include <franka_hw/control_mode.h>
 #include <franka_hw/franka_cartesian_command_interface.h>
 #include <franka_hw/franka_model_interface.h>
 #include <franka_hw/franka_state_interface.h>
+#include <franka_hw/services.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 
-using franka_control::ServiceContainer;
+using franka_hw::ServiceContainer;
 using franka_hw::ControlMode;
 
 namespace franka_combinable_hw {
@@ -302,7 +302,7 @@ class FrankaCombinableHW : public hardware_interface::RobotHW {
 
   ros::Publisher has_error_pub_;
   ServiceContainer services_;
-  std::unique_ptr<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>
+  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
       recovery_action_server_;
 
   std::atomic<bool> has_error_;

diff --git a/franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h b/franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h
@@ -4,7 +4,7 @@
 
 #include <combined_robot_hw/combined_robot_hw.h>
 #include <franka_combinable_hw/franka_combinable_hw.h>
-#include <franka_control/ErrorRecoveryAction.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 
 #include <actionlib/server/simple_action_server.h>
 #include <ros/node_handle.h>
@@ -51,7 +51,7 @@ class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW {
   bool controllerNeedsReset();
 
  protected:
-  std::unique_ptr<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>
+  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
       combined_recovery_action_server_;
 
  private:

diff --git a/franka_combinable_hw/src/franka_combinable_hw.cpp b/franka_combinable_hw/src/franka_combinable_hw.cpp
@@ -5,11 +5,11 @@
 #include <franka/exception.h>
 #include <franka/rate_limiting.h>
 #include <franka_combinable_hw/franka_combinable_hw.h>
-#include <franka_control/ErrorRecoveryAction.h>
 #include <franka_hw/franka_cartesian_command_interface.h>
 #include <franka_hw/franka_model_interface.h>
 #include <franka_hw/franka_state_interface.h>
 #include <franka_hw/resource_helpers.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 
 #include <pluginlib/class_list_macros.h>
 
@@ -20,7 +20,7 @@
 #include <joint_limits_interface/joint_limits_urdf.h>
 #include <std_msgs/Bool.h>
 
-using franka_control::ErrorRecoveryResult;
+using franka_msgs::ErrorRecoveryResult;
 using franka_hw::ArmClaimedMap;
 using franka_hw::getResourceMap;
 using franka_hw::ResourceWithClaimsMap;
@@ -316,45 +316,44 @@ void FrankaCombinableHW::controlLoop() {
 
 void FrankaCombinableHW::setupServicesAndActionServers(ros::NodeHandle& node_handle) {
   services_
-      .advertiseService<franka_control::SetJointImpedance>(
-          node_handle, "set_joint_impedance",
-          [this](auto&& req, auto&& res) {
-            return franka_control::setJointImpedance(*(this->robot_), req, res);
-          })
-      .advertiseService<franka_control::SetCartesianImpedance>(
+      .advertiseService<franka_msgs::SetJointImpedance>(node_handle, "set_joint_impedance",
+                                                        [this](auto&& req, auto&& res) {
+                                                          return franka_hw::setJointImpedance(
+                                                              *(this->robot_), req, res);
+                                                        })
+      .advertiseService<franka_msgs::SetCartesianImpedance>(
           node_handle, "set_cartesian_impedance",
           [this](auto&& req, auto&& res) {
-            return franka_control::setCartesianImpedance(*(this->robot_), req, res);
+            return franka_hw::setCartesianImpedance(*(this->robot_), req, res);
           })
-      .advertiseService<franka_control::SetEEFrame>(node_handle, "set_EE_frame",
-                                                    [this](auto&& req, auto&& res) {
-                                                      return franka_control::setEEFrame(
-                                                          *(this->robot_), req, res);
-                                                    })
-      .advertiseService<franka_control::SetKFrame>(node_handle, "set_K_frame",
-                                                   [this](auto&& req, auto&& res) {
-                                                     return franka_control::setKFrame(
-                                                         *(this->robot_), req, res);
-                                                   })
-      .advertiseService<franka_control::SetForceTorqueCollisionBehavior>(
+      .advertiseService<franka_msgs::SetEEFrame>(node_handle, "set_EE_frame",
+                                                 [this](auto&& req, auto&& res) {
+                                                   return franka_hw::setEEFrame(*(this->robot_),
+                                                                                req, res);
+                                                 })
+      .advertiseService<franka_msgs::SetKFrame>(node_handle, "set_K_frame",
+                                                [this](auto&& req, auto&& res) {
+                                                  return franka_hw::setKFrame(*(this->robot_), req,
+                                                                              res);
+                                                })
+      .advertiseService<franka_msgs::SetForceTorqueCollisionBehavior>(
           node_handle, "set_force_torque_collision_behavior",
           [this](auto&& req, auto&& res) {
-            return franka_control::setForceTorqueCollisionBehavior(*(this->robot_), req, res);
+            return franka_hw::setForceTorqueCollisionBehavior(*(this->robot_), req, res);
           })
-      .advertiseService<franka_control::SetFullCollisionBehavior>(
+      .advertiseService<franka_msgs::SetFullCollisionBehavior>(
           node_handle, "set_full_collision_behavior",
           [this](auto&& req, auto&& res) {
-            return franka_control::setFullCollisionBehavior(*(this->robot_), req, res);
+            return franka_hw::setFullCollisionBehavior(*(this->robot_), req, res);
           })
-      .advertiseService<franka_control::SetLoad>(
-          node_handle, "set_load", [this](auto&& req, auto&& res) {
-            return franka_control::setLoad(*(this->robot_), req, res);
-          });
+      .advertiseService<franka_msgs::SetLoad>(
+          node_handle, "set_load",
+          [this](auto&& req, auto&& res) { return franka_hw::setLoad(*(this->robot_), req, res); });
 
   recovery_action_server_ =
-      std::make_unique<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>(
+      std::make_unique<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>(
           node_handle, "error_recovery",
-          [&](const franka_control::ErrorRecoveryGoalConstPtr&) {
+          [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {
             try {
               robot_->automaticErrorRecovery();
               // error recovered => reset controller

diff --git a/franka_combinable_hw/src/franka_combined_hw.cpp b/franka_combinable_hw/src/franka_combined_hw.cpp
@@ -3,8 +3,8 @@
 #include <franka_combinable_hw/franka_combinable_hw.h>
 
 #include <franka_combinable_hw/franka_combined_hw.h>
-#include <franka_control/ErrorRecoveryAction.h>
 #include <franka_hw/franka_hw.h>
+#include <franka_msgs/ErrorRecoveryAction.h>
 
 #include <actionlib/server/simple_action_server.h>
 #include <ros/node_handle.h>
@@ -21,9 +21,9 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_
   bool success = CombinedRobotHW::init(root_nh, robot_hw_nh);
   // Error recovery server for all FrankaHWs
   combined_recovery_action_server_ =
-      std::make_unique<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>(
+      std::make_unique<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>(
           robot_hw_nh, "error_recovery",
-          [&](const franka_control::ErrorRecoveryGoalConstPtr&) {
+          [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {
             try {
               is_recovering_ = true;
               for (const auto& robot_hw : robot_hw_list_) {
@@ -36,7 +36,7 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_
                       "FrankaCombinedHW: dynamic_cast from RobotHW to FrankaCombinableHW failed.");
                   is_recovering_ = false;
                   combined_recovery_action_server_->setAborted(
-                      franka_control::ErrorRecoveryResult(),
+                      franka_msgs::ErrorRecoveryResult(),
                       "dynamic_cast from RobotHW to FrankaCombinableHW failed");
                   return;
                 }
@@ -45,7 +45,7 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_
               combined_recovery_action_server_->setSucceeded();
             } catch (const franka::Exception& ex) {
               is_recovering_ = false;
-              combined_recovery_action_server_->setAborted(franka_control::ErrorRecoveryResult(),
+              combined_recovery_action_server_->setAborted(franka_msgs::ErrorRecoveryResult(),
                                                            ex.what());
             }
           },

diff --git a/franka_control/CMakeLists.txt b/franka_control/CMakeLists.txt
@@ -5,8 +5,6 @@ set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 find_package(catkin REQUIRED COMPONENTS
-  actionlib
-  actionlib_msgs
   controller_interface
   controller_manager
   franka_hw
@@ -23,32 +21,14 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Franka 0.5.0 REQUIRED)
 
-add_service_files(FILES
-  SetCartesianImpedance.srv
-  SetEEFrame.srv
-  SetForceTorqueCollisionBehavior.srv
-  SetFullCollisionBehavior.srv
-  SetJointImpedance.srv
-  SetKFrame.srv
-  SetLoad.srv
-)
-
-add_action_files(FILES
-  ErrorRecovery.action
-)
-
-generate_messages(DEPENDENCIES actionlib_msgs)
-
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES franka_state_controller franka_control_services
   CATKIN_DEPENDS
-    actionlib
     controller_interface
     franka_hw
     franka_msgs
     geometry_msgs
-    message_runtime
     pluginlib
     realtime_tools
     roscpp
@@ -80,29 +60,6 @@ target_include_directories(franka_state_controller PUBLIC
   include
 )
 
-## franka_control_services
-add_library(franka_control_services
-  src/services.cpp
-)
-
-add_dependencies(franka_control_services
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(franka_control_services
-  ${Franka_LIBRARIES}
-  ${catkin_LIBRARIES}
-)
-
-target_include_directories(franka_control_services SYSTEM PUBLIC
-  ${Franka_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-)
-target_include_directories(franka_control_services PUBLIC
-  include
-)
-
 ## franka_control_node
 add_executable(franka_control_node
   src/franka_control_node.cpp
@@ -126,7 +83,6 @@ target_include_directories(franka_control_node SYSTEM PUBLIC
 
 ## Installation
 install(TARGETS franka_state_controller
-                franka_control_services
                 franka_control_node
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

diff --git a/franka_control/include/franka_control/services.h b/franka_control/include/franka_control/services.h
diff --git a/franka_control/package.xml b/franka_control/package.xml
@@ -13,11 +13,7 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>message_generation</build_depend>
-
   <depend>libfranka</depend>
-  <depend>actionlib</depend>
-  <depend>actionlib_msgs</depend>
   <depend>controller_interface</depend>
   <depend>controller_manager</depend>
   <depend>franka_hw</depend>
@@ -33,11 +29,8 @@
   <exec_depend>franka_description</exec_depend>
   <exec_depend>franka_gripper</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
 
-  <build_export_depend>message_runtime</build_export_depend>
-
   <export>
     <controller_interface plugin="${prefix}/franka_controller_plugins.xml"/>
   </export>