Merge pull request frankaemika#38 in SWDEV/franka_ros from SWDEV-304-…

…libfranka-clean-up-public-api to master * commit '67b0334b72fc0c181deaac1c612e81a023c86570': clang-format. Rename move to control. Change order of callbacks for control calls in API. Rename motion generator 'control' function to 'move'. Clang-format Remove setTimeScalingFactor. Change franka::Duration members: s -> toSec.
erdalpekel · Sep 11, 2017 · bf5bc2a · bf5bc2a
2 parents 2941ea4 + 6b9ba3e
commit bf5bc2a
Show file tree

Hide file tree

Showing 7 changed files with 20 additions and 38 deletions.
diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt
@@ -41,7 +41,6 @@ add_service_files(FILES
   SetJointImpedance.srv
   SetKFrame.srv
   SetLoad.srv
-  SetTimeScalingFactor.srv
 )
 
 add_action_files(FILES

diff --git a/franka_hw/include/franka_hw/services.h b/franka_hw/include/franka_hw/services.h
@@ -13,7 +13,6 @@
 #include <franka_hw/SetJointImpedance.h>
 #include <franka_hw/SetKFrame.h>
 #include <franka_hw/SetLoad.h>
-#include <franka_hw/SetTimeScalingFactor.h>
 
 namespace franka_hw {
 namespace services {
@@ -53,9 +52,6 @@ void setFullCollisionBehavior(franka::Robot& robot,
                               const SetFullCollisionBehavior::Request& req,
                               SetFullCollisionBehavior::Response& res);
 void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& res);
-void setTimeScalingFactor(franka::Robot& robot,
-                          const SetTimeScalingFactor::Request& req,
-                          SetTimeScalingFactor::Response& res);
 
 }  // namespace services
 }  // namespace franka_hw
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
@@ -146,7 +146,7 @@ void FrankaHW::control(franka::Robot& robot,
                                                         franka::Duration time_step) {
     if (last_time != robot_state.time) {
       last_time = robot_state.time;
-      return ros_callback(ros::Time::now(), ros::Duration(time_step.s()));
+      return ros_callback(ros::Time::now(), ros::Duration(time_step.toSec()));
     }
     return true;
   });
@@ -293,34 +293,34 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
       break;
     case (ControlMode::JointTorque | ControlMode::JointPosition):
       run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
-        robot.control(std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this,
-                                std::cref(position_joint_command_), ros_callback, _1, _2),
-                      std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2));
+        robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
+                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                      std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this,
+                                std::cref(position_joint_command_), ros_callback, _1, _2));
       };
       break;
     case (ControlMode::JointTorque | ControlMode::JointVelocity):
       run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
-        robot.control(std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this,
-                                std::cref(velocity_joint_command_), ros_callback, _1, _2),
-                      std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2));
+        robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
+                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                      std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this,
+                                std::cref(velocity_joint_command_), ros_callback, _1, _2));
       };
       break;
     case (ControlMode::JointTorque | ControlMode::CartesianPose):
       run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
-        robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this,
-                                std::cref(pose_cartesian_command_), ros_callback, _1, _2),
-                      std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2));
+        robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
+                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                      std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this,
+                                std::cref(pose_cartesian_command_), ros_callback, _1, _2));
       };
       break;
     case (ControlMode::JointTorque | ControlMode::CartesianVelocity):
       run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
-        robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
-                                std::cref(velocity_cartesian_command_), ros_callback, _1, _2),
-                      std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
-                                std::cref(effort_joint_command_), ros_callback, _1, _2));
+        robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
+                                std::cref(effort_joint_command_), ros_callback, _1, _2),
+                      std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
+                                std::cref(velocity_cartesian_command_), ros_callback, _1, _2));
       };
       break;
     default:

diff --git a/franka_hw/src/franka_hw_node.cpp b/franka_hw/src/franka_hw_node.cpp
@@ -75,10 +75,8 @@ int main(int argc, char** argv) {
           node_handle, "set_full_collision_behavior",
           std::bind(franka_hw::services::setFullCollisionBehavior, std::ref(robot), _1, _2))
       .advertiseService<franka_hw::SetLoad>(
-          node_handle, "set_load", std::bind(franka_hw::services::setLoad, std::ref(robot), _1, _2))
-      .advertiseService<franka_hw::SetTimeScalingFactor>(
-          node_handle, "set_time_scaling_factor",
-          std::bind(franka_hw::services::setTimeScalingFactor, std::ref(robot), _1, _2));
+          node_handle, "set_load",
+          std::bind(franka_hw::services::setLoad, std::ref(robot), _1, _2));
 
   actionlib::SimpleActionServer<franka_hw::ErrorRecoveryAction> recovery_action_server(
       node_handle, "error_recovery",

diff --git a/franka_hw/src/franka_state_controller.cpp b/franka_hw/src/franka_state_controller.cpp
@@ -302,7 +302,7 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) {
       publisher_franka_states_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];
     }
 
-    publisher_franka_states_.msg_.time = robot_state_.time.s();
+    publisher_franka_states_.msg_.time = robot_state_.time.toSec();
     publisher_franka_states_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);
     publisher_franka_states_.msg_.last_motion_errors =
         errorsToMessage(robot_state_.last_motion_errors);

diff --git a/franka_hw/src/services.cpp b/franka_hw/src/services.cpp
@@ -105,11 +105,5 @@ void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Respons
   robot.setLoad(mass, F_x_center_load, load_inertia);
 }
 
-void setTimeScalingFactor(franka::Robot& robot,
-                          const SetTimeScalingFactor::Request& req,
-                          SetTimeScalingFactor::Response& /* res */) {
-  robot.setTimeScalingFactor(req.time_scaling_factor);
-}
-
 }  // namespace services
 }  // namespace franka_hw
diff --git a/franka_hw/srv/SetTimeScalingFactor.srv b/franka_hw/srv/SetTimeScalingFactor.srv