diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt
index 99b7e9b58303af528229ac584e89e94d2785ef83..a85969abdc2ba71dd3ab785a0762f1910733e157 100644
--- 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
index 451c5db7d1ae53b92f9f2efd29fc86888b99e2fd..5179109136709b185bd3a88426f413a79dd715e9 100644
--- 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
index 07bc87162abbd31dda42ed73b6885aa5839d09d9..a9f3f18f03b785fef007a4b4f9db9eac46250fb7 100644
--- 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
index 04616950aed0cfad4dffecf6c9198f0916301d42..65b9965157e683d1e4a5be240eaa9040fd2403c8 100644
--- 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
index 1f1a68302ae4a4927c43732ec51ef6de1379e290..80f67dd6a28627cb3878d02705f17a0716ad4b28 100644
--- 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
index c334b157b2faff7c3a7a3e0688d084a1c4e217be..71360e085f70415d81490dd0ce92be4e332029c4 100644
--- 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
deleted file mode 100644
index 92fdee2c7a2575df3e21d698a4bdab3ed133b62b..0000000000000000000000000000000000000000
--- a/franka_hw/srv/SetTimeScalingFactor.srv
+++ /dev/null
@@ -1,5 +0,0 @@
-float64 time_scaling_factor
----
-bool success
-string error
-