diff --git a/franka_control/include/franka_control/franka_state_controller.h b/franka_control/include/franka_control/franka_state_controller.h index 18297111b1d64633cd26735b83c0ab0e21522a7b..6fe42947309ba715bbc052c00349d42a78307cbc 100644 --- a/franka_control/include/franka_control/franka_state_controller.h +++ b/franka_control/include/franka_control/franka_state_controller.h @@ -26,7 +26,7 @@ class FrankaStateController /** * Creates an instance of a FrankaStateController. */ - FrankaStateController(); + FrankaStateController() = default; /** * Initializes the controller with interfaces and publishers. @@ -55,8 +55,8 @@ class FrankaStateController std::string arm_id_; - franka_hw::FrankaStateInterface* franka_state_interface_; - std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_; + franka_hw::FrankaStateInterface* franka_state_interface_{}; + std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_{}; realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_; realtime_tools::RealtimePublisher<franka_msgs::FrankaState> publisher_franka_states_; diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp index 45acdf1741e91390a5d31662a6fde510f1517249..ce116d5e7983749afe4db99afe3fa052e5e66d8b 100644 --- a/franka_control/src/franka_control_node.cpp +++ b/franka_control/src/franka_control_node.cpp @@ -89,29 +89,37 @@ int main(int argc, char** argv) { std::atomic_bool has_error(false); - using std::placeholders::_1; - using std::placeholders::_2; ServiceContainer services; services .advertiseService<franka_control::SetJointImpedance>( node_handle, "set_joint_impedance", - std::bind(franka_control::setJointImpedance, std::ref(robot), _1, _2)) + [&robot](auto&& req, auto&& res) { + return franka_control::setJointImpedance(robot, req, res); + }) .advertiseService<franka_control::SetCartesianImpedance>( node_handle, "set_cartesian_impedance", - std::bind(franka_control::setCartesianImpedance, std::ref(robot), _1, _2)) + [&robot](auto&& req, auto&& res) { + return franka_control::setCartesianImpedance(robot, req, res); + }) .advertiseService<franka_control::SetEEFrame>( node_handle, "set_EE_frame", - std::bind(franka_control::setEEFrame, std::ref(robot), _1, _2)) + [&robot](auto&& req, auto&& res) { return franka_control::setEEFrame(robot, req, res); }) .advertiseService<franka_control::SetKFrame>( - node_handle, "set_K_frame", std::bind(franka_control::setKFrame, std::ref(robot), _1, _2)) + node_handle, "set_K_frame", + [&robot](auto&& req, auto&& res) { return franka_control::setKFrame(robot, req, res); }) .advertiseService<franka_control::SetForceTorqueCollisionBehavior>( node_handle, "set_force_torque_collision_behavior", - std::bind(franka_control::setForceTorqueCollisionBehavior, std::ref(robot), _1, _2)) + [&robot](auto&& req, auto&& res) { + return franka_control::setForceTorqueCollisionBehavior(robot, req, res); + }) .advertiseService<franka_control::SetFullCollisionBehavior>( node_handle, "set_full_collision_behavior", - std::bind(franka_control::setFullCollisionBehavior, std::ref(robot), _1, _2)) + [&robot](auto&& req, auto&& res) { + return franka_control::setFullCollisionBehavior(robot, req, res); + }) .advertiseService<franka_control::SetLoad>( - node_handle, "set_load", std::bind(franka_control::setLoad, std::ref(robot), _1, _2)); + node_handle, "set_load", + [&robot](auto&& req, auto&& res) { return franka_control::setLoad(robot, req, res); }); actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction> recovery_action_server( node_handle, "error_recovery", diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp index 457e2adce3d0a5afa2e676e32ea78b3a4e1efceb..2e1e510f4bd236a9b63557f9d036d849d350fe61 100644 --- a/franka_control/src/franka_state_controller.cpp +++ b/franka_control/src/franka_state_controller.cpp @@ -3,6 +3,7 @@ #include <franka_control/franka_state_controller.h> #include <cmath> +#include <memory> #include <mutex> #include <string> @@ -134,16 +135,6 @@ franka_msgs::Errors errorsToMessage(const franka::Errors& error) { namespace franka_control { -FrankaStateController::FrankaStateController() - : franka_state_interface_(nullptr), - franka_state_handle_(nullptr), - publisher_transforms_(), - publisher_franka_states_(), - publisher_joint_states_(), - publisher_joint_states_desired_(), - publisher_external_wrench_(), - trigger_publish_(30.0) {} - bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, ros::NodeHandle& controller_node_handle) { @@ -157,12 +148,11 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware, return false; } double publish_rate(30.0); - if (controller_node_handle.getParam("publish_rate", publish_rate)) { - trigger_publish_ = franka_hw::TriggerRate(publish_rate); - } else { + if (!controller_node_handle.getParam("publish_rate", publish_rate)) { ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default " << publish_rate << " [Hz]."); } + trigger_publish_ = franka_hw::TriggerRate(publish_rate); if (!controller_node_handle.getParam("joint_names", joint_names_) || joint_names_.size() != robot_state_.q.size()) { @@ -173,8 +163,8 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware, } try { - franka_state_handle_.reset( - new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id_ + "_robot"))); + franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>( + franka_state_interface_->getHandle(arm_id_ + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM("FrankaStateController: Exception getting franka state handle: " << ex.what()); return false; diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 84afb873ed47526ea933c7f3da08c238e152e3cd..1d9120adc3f0e5eb0cca2bff66ce58a188cd0a5b 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -3,6 +3,7 @@ #include <franka_example_controllers/cartesian_impedance_example_controller.h> #include <cmath> +#include <memory> #include <controller_interface/controller_base.h> #include <franka/robot_state.h> @@ -35,16 +36,15 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo return false; } - franka_hw::FrankaModelInterface* model_interface = - robot_hw->get<franka_hw::FrankaModelInterface>(); + auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>(); if (model_interface == nullptr) { ROS_ERROR_STREAM( "CartesianImpedanceExampleController: Error getting model interface from hardware"); return false; } try { - model_handle_.reset( - new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model"))); + model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>( + model_interface->getHandle(arm_id + "_model")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "CartesianImpedanceExampleController: Exception getting model handle from interface: " @@ -52,16 +52,15 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo return false; } - franka_hw::FrankaStateInterface* state_interface = - robot_hw->get<franka_hw::FrankaStateInterface>(); + auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>(); if (state_interface == nullptr) { ROS_ERROR_STREAM( "CartesianImpedanceExampleController: Error getting state interface from hardware"); return false; } try { - state_handle_.reset( - new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot"))); + state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>( + state_interface->getHandle(arm_id + "_robot")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "CartesianImpedanceExampleController: Exception getting state handle from interface: " @@ -69,8 +68,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo return false; } - hardware_interface::EffortJointInterface* effort_joint_interface = - robot_hw->get<hardware_interface::EffortJointInterface>(); + auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>(); if (effort_joint_interface == nullptr) { ROS_ERROR_STREAM( "CartesianImpedanceExampleController: Error getting effort joint interface from hardware"); @@ -89,9 +87,10 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo dynamic_reconfigure_compliance_param_node_ = ros::NodeHandle("dynamic_reconfigure_compliance_param_node"); - dynamic_server_compliance_param_.reset( - new dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>( - dynamic_reconfigure_compliance_param_node_)); + dynamic_server_compliance_param_ = std::make_unique< + dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>( + + dynamic_reconfigure_compliance_param_node_); dynamic_server_compliance_param_->setCallback( boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2)); @@ -114,9 +113,9 @@ void CartesianImpedanceExampleController::starting(const ros::Time& /*time*/) { std::array<double, 42> jacobian_array = model_handle_->getZeroJacobian(franka::Frame::kEndEffector); // convert to eigen - Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > dq_initial(initial_state.dq.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > q_initial(initial_state.q.data()); + Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> dq_initial(initial_state.dq.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> q_initial(initial_state.q.data()); Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())); // set equilibrium point to current state @@ -138,11 +137,11 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, model_handle_->getZeroJacobian(franka::Frame::kEndEffector); // convert to Eigen - Eigen::Map<Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data()); - Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > q(robot_state.q.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d( // NOLINT (readability-identifier-naming) + Eigen::Map<Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data()); + Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> q(robot_state.q.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming) robot_state.tau_J_d.data()); Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); Eigen::Vector3d position(transform.translation()); diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp index ae1b787aa97fe89d8ce80a028a02a698600b95d4..af69500f3b79d783fd08f8361c63f42da69c9e2e 100644 --- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp @@ -3,6 +3,7 @@ #include <franka_example_controllers/cartesian_pose_example_controller.h> #include <cmath> +#include <memory> #include <stdexcept> #include <string> @@ -31,8 +32,8 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har } try { - cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle( - cartesian_pose_interface_->getHandle(arm_id + "_robot"))); + cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>( + cartesian_pose_interface_->getHandle(arm_id + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& e) { ROS_ERROR_STREAM( "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what()); diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp index 59e0f6fedbf02a3d85d5884e74c36780694c8d95..67c34a255b04180b5cb4cfb5d48c003b4637705e 100644 --- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp @@ -4,6 +4,7 @@ #include <array> #include <cmath> +#include <memory> #include <string> #include <controller_interface/controller_base.h> @@ -31,8 +32,8 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot return false; } try { - velocity_cartesian_handle_.reset(new franka_hw::FrankaCartesianVelocityHandle( - velocity_cartesian_interface_->getHandle(arm_id + "_robot"))); + velocity_cartesian_handle_ = std::make_unique<franka_hw::FrankaCartesianVelocityHandle>( + velocity_cartesian_interface_->getHandle(arm_id + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& e) { ROS_ERROR_STREAM( "CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what()); diff --git a/franka_example_controllers/src/elbow_example_controller.cpp b/franka_example_controllers/src/elbow_example_controller.cpp index f12137ebd6f75927d16de71ad6a13bb2a47d2290..c988cb502164eb85ef0a803f9347b58a28ab625a 100644 --- a/franka_example_controllers/src/elbow_example_controller.cpp +++ b/franka_example_controllers/src/elbow_example_controller.cpp @@ -3,6 +3,7 @@ #include <franka_example_controllers/elbow_example_controller.h> #include <cmath> +#include <memory> #include <stdexcept> #include <string> @@ -29,8 +30,8 @@ bool ElbowExampleController::init(hardware_interface::RobotHW* robot_hardware, } try { - cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle( - cartesian_pose_interface_->getHandle(arm_id + "_robot"))); + cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>( + cartesian_pose_interface_->getHandle(arm_id + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& e) { ROS_ERROR_STREAM("ElbowExampleController: Exception getting Cartesian handle: " << e.what()); return false; diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp index 4e49c2f6ef78ccc01e65e8c1d8870657c5bbaed0..36cce5da7fb74c1f018240ba22615fd88c75e8eb 100644 --- a/franka_example_controllers/src/force_example_controller.cpp +++ b/franka_example_controllers/src/force_example_controller.cpp @@ -3,6 +3,7 @@ #include <franka_example_controllers/force_example_controller.h> #include <cmath> +#include <memory> #include <controller_interface/controller_base.h> #include <pluginlib/class_list_macros.h> @@ -30,38 +31,35 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw, return false; } - franka_hw::FrankaModelInterface* model_interface = - robot_hw->get<franka_hw::FrankaModelInterface>(); + auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>(); if (model_interface == nullptr) { ROS_ERROR_STREAM("ForceExampleController: Error getting model interface from hardware"); return false; } try { - model_handle_.reset( - new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model"))); + model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>( + model_interface->getHandle(arm_id + "_model")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "ForceExampleController: Exception getting model handle from interface: " << ex.what()); return false; } - franka_hw::FrankaStateInterface* state_interface = - robot_hw->get<franka_hw::FrankaStateInterface>(); + auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>(); if (state_interface == nullptr) { ROS_ERROR_STREAM("ForceExampleController: Error getting state interface from hardware"); return false; } try { - state_handle_.reset( - new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot"))); + state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>( + state_interface->getHandle(arm_id + "_robot")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "ForceExampleController: Exception getting state handle from interface: " << ex.what()); return false; } - hardware_interface::EffortJointInterface* effort_joint_interface = - robot_hw->get<hardware_interface::EffortJointInterface>(); + auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>(); if (effort_joint_interface == nullptr) { ROS_ERROR_STREAM("ForceExampleController: Error getting effort joint interface from hardware"); return false; @@ -77,9 +75,10 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw, dynamic_reconfigure_desired_mass_param_node_ = ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node"); - dynamic_server_desired_mass_param_.reset( - new dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>( - dynamic_reconfigure_desired_mass_param_node_)); + dynamic_server_desired_mass_param_ = std::make_unique< + dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>( + + dynamic_reconfigure_desired_mass_param_node_); dynamic_server_desired_mass_param_->setCallback( boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2)); @@ -89,8 +88,8 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw, void ForceExampleController::starting(const ros::Time& /*time*/) { franka::RobotState robot_state = state_handle_->getRobotState(); std::array<double, 7> gravity_array = model_handle_->getGravity(); - Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data()); // Bias correction for the current external torque tau_ext_initial_ = tau_measured - gravity; tau_error_.setZero(); @@ -101,11 +100,11 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati std::array<double, 42> jacobian_array = model_handle_->getZeroJacobian(franka::Frame::kEndEffector); std::array<double, 7> gravity_array = model_handle_->getGravity(); - Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d( // NOLINT (readability-identifier-naming) + Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming) robot_state.tau_J_d.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data()); + Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data()); Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7); desired_force_torque.setZero(); diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp index bfb075e2873fc056dc60b65320cf43dcdfb7ac85..607e87da9d919f03cef4925f46b92c8b9ab5e1da 100644 --- a/franka_example_controllers/src/joint_impedance_example_controller.cpp +++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp @@ -3,6 +3,7 @@ #include <franka_example_controllers/joint_impedance_example_controller.h> #include <cmath> +#include <memory> #include <controller_interface/controller_base.h> #include <pluginlib/class_list_macros.h> @@ -72,16 +73,15 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw << coriolis_factor_); } - franka_hw::FrankaModelInterface* model_interface = - robot_hw->get<franka_hw::FrankaModelInterface>(); + auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>(); if (model_interface == nullptr) { ROS_ERROR_STREAM( "JointImpedanceExampleController: Error getting model interface from hardware"); return false; } try { - model_handle_.reset( - new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model"))); + model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>( + model_interface->getHandle(arm_id + "_model")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "JointImpedanceExampleController: Exception getting model handle from interface: " @@ -89,16 +89,15 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw return false; } - franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface = - robot_hw->get<franka_hw::FrankaPoseCartesianInterface>(); + auto* cartesian_pose_interface = robot_hw->get<franka_hw::FrankaPoseCartesianInterface>(); if (cartesian_pose_interface == nullptr) { ROS_ERROR_STREAM( "JointImpedanceExampleController: Error getting cartesian pose interface from hardware"); return false; } try { - cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle( - cartesian_pose_interface->getHandle(arm_id + "_robot"))); + cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>( + cartesian_pose_interface->getHandle(arm_id + "_robot")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "JointImpedanceExampleController: Exception getting cartesian pose handle from interface: " @@ -106,8 +105,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw return false; } - hardware_interface::EffortJointInterface* effort_joint_interface = - robot_hw->get<hardware_interface::EffortJointInterface>(); + auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>(); if (effort_joint_interface == nullptr) { ROS_ERROR_STREAM( "JointImpedanceExampleController: Error getting effort joint interface from hardware"); diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp index 218855945ae47a92a2bab03ecd4b24729060da14..f9ccea429ad6b0c9ebd47c8f4d704c0e6ec47ff2 100644 --- a/franka_example_controllers/src/model_example_controller.cpp +++ b/franka_example_controllers/src/model_example_controller.cpp @@ -7,6 +7,7 @@ #include <array> #include <cstring> #include <iterator> +#include <memory> #include <controller_interface/controller_base.h> #include <hardware_interface/hardware_interface.h> @@ -48,8 +49,8 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw, } try { - franka_state_handle_.reset( - new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id + "_robot"))); + franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>( + franka_state_interface_->getHandle(arm_id + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "ModelExampleController: Exception getting franka state handle: " << ex.what()); @@ -57,8 +58,8 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw, } try { - model_handle_.reset( - new franka_hw::FrankaModelHandle(model_interface_->getHandle(arm_id + "_model"))); + model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>( + model_interface_->getHandle(arm_id + "_model")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "ModelExampleController: Exception getting model handle from interface: " << ex.what()); diff --git a/franka_gripper/src/franka_gripper_node.cpp b/franka_gripper/src/franka_gripper_node.cpp index d4266a0b0f4c4464fc6c66a3ecda1db82cd558a4..64a8d254854320e7f348159a5d53d5f3fc1688c6 100644 --- a/franka_gripper/src/franka_gripper_node.cpp +++ b/franka_gripper/src/franka_gripper_node.cpp @@ -84,49 +84,56 @@ int main(int argc, char** argv) { franka::Gripper gripper(robot_ip); - std::function<bool(const HomingGoalConstPtr&)> homing_handler = - std::bind(homing, std::cref(gripper), std::placeholders::_1); - std::function<bool(const StopGoalConstPtr&)> stop_handler = - std::bind(stop, std::cref(gripper), std::placeholders::_1); - std::function<bool(const GraspGoalConstPtr&)> grasp_handler = - std::bind(grasp, std::cref(gripper), std::placeholders::_1); - std::function<bool(const MoveGoalConstPtr&)> move_handler = - std::bind(move, std::cref(gripper), std::placeholders::_1); - - SimpleActionServer<HomingAction> homing_action_server_( + auto homing_handler = [&gripper](auto&& goal) { return homing(gripper, goal); }; + auto stop_handler = [&gripper](auto&& goal) { return stop(gripper, goal); }; + auto grasp_handler = [&gripper](auto&& goal) { return grasp(gripper, goal); }; + auto move_handler = [&gripper](auto&& goal) { return move(gripper, goal); }; + + SimpleActionServer<HomingAction> homing_action_server( node_handle, "homing", - std::bind(handleErrors<HomingAction, HomingGoalConstPtr, HomingResult>, - &homing_action_server_, homing_handler, std::placeholders::_1), + [=, &homing_action_server](auto&& goal) { + return handleErrors<franka_gripper::HomingAction, franka_gripper::HomingGoalConstPtr, + franka_gripper::HomingResult>(&homing_action_server, homing_handler, + goal); + }, false); - SimpleActionServer<StopAction> stop_action_server_( + SimpleActionServer<StopAction> stop_action_server( node_handle, "stop", - std::bind(handleErrors<StopAction, StopGoalConstPtr, StopResult>, &stop_action_server_, - stop_handler, std::placeholders::_1), + [=, &stop_action_server](auto&& goal) { + return handleErrors<franka_gripper::StopAction, franka_gripper::StopGoalConstPtr, + franka_gripper::StopResult>(&stop_action_server, stop_handler, goal); + }, false); - SimpleActionServer<MoveAction> move_action_server_( + SimpleActionServer<MoveAction> move_action_server( node_handle, "move", - std::bind(handleErrors<MoveAction, MoveGoalConstPtr, MoveResult>, &move_action_server_, - move_handler, std::placeholders::_1), + [=, &move_action_server](auto&& goal) { + return handleErrors<franka_gripper::MoveAction, franka_gripper::MoveGoalConstPtr, + franka_gripper::MoveResult>(&move_action_server, move_handler, goal); + }, false); - SimpleActionServer<GraspAction> grasp_action_server_( + SimpleActionServer<GraspAction> grasp_action_server( node_handle, "grasp", - std::bind(handleErrors<GraspAction, GraspGoalConstPtr, GraspResult>, &grasp_action_server_, - grasp_handler, std::placeholders::_1), + [=, &grasp_action_server](auto&& goal) { + return handleErrors<franka_gripper::GraspAction, franka_gripper::GraspGoalConstPtr, + franka_gripper::GraspResult>(&grasp_action_server, grasp_handler, goal); + }, false); SimpleActionServer<GripperCommandAction> gripper_command_action_server( node_handle, "gripper_action", - std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_grasp_epsilon, - default_speed, &gripper_command_action_server, std::placeholders::_1), + [=, &gripper, &gripper_command_action_server](auto&& goal) { + return gripperCommandExecuteCallback(gripper, default_grasp_epsilon, default_speed, + &gripper_command_action_server, goal); + }, false); - homing_action_server_.start(); - stop_action_server_.start(); - move_action_server_.start(); - grasp_action_server_.start(); + homing_action_server.start(); + stop_action_server.start(); + move_action_server.start(); + grasp_action_server.start(); gripper_command_action_server.start(); double publish_rate(30.0); diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h index e99003640f62acf90bb7e6e9f40a628595330aaa..52f7610b60ba7189725bd6e30da3fb81b123386a 100644 --- a/franka_hw/include/franka_hw/franka_hw.h +++ b/franka_hw/include/franka_hw/franka_hw.h @@ -93,7 +93,7 @@ class FrankaHW : public hardware_interface::RobotHW { * @throw franka::RealtimeException if realtime priority cannot be set for the current thread. */ void control(franka::Robot& robot, - std::function<bool(const ros::Time&, const ros::Duration&)> ros_callback); + const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback); /** * Updates the controller interfaces from the given robot state. diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp index ddad4d90cb813cf344f0a10febad56227b48cfa5..50f20d02d8fd24ac68805612a5443f611061acf5 100644 --- a/franka_hw/src/franka_hw.cpp +++ b/franka_hw/src/franka_hw.cpp @@ -4,6 +4,7 @@ #include <cstdint> #include <ostream> +#include <utility> #include <franka/rate_limiting.h> #include <joint_limits_interface/joint_limits_urdf.h> @@ -32,9 +33,9 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names, std::function<franka::ControllerMode()> get_internal_controller) : joint_names_(joint_names), arm_id_(arm_id), - get_internal_controller_(get_internal_controller), - get_limit_rate_(get_limit_rate), - get_cutoff_frequency_(get_cutoff_frequency), + get_internal_controller_(std::move(get_internal_controller)), + get_limit_rate_(std::move(get_limit_rate)), + get_cutoff_frequency_(std::move(get_cutoff_frequency)), position_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), velocity_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), effort_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), @@ -137,9 +138,9 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names, : FrankaHW(joint_names, arm_id, urdf_model, - get_limit_rate, - get_cutoff_frequency, - get_internal_controller) { + std::move(get_limit_rate), + std::move(get_cutoff_frequency), + std::move(get_internal_controller)) { franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", model, robot_state_); franka_model_interface_.registerHandle(model_handle); @@ -155,8 +156,9 @@ bool FrankaHW::controllerActive() const noexcept { return controller_active_; } -void FrankaHW::control(franka::Robot& robot, - std::function<bool(const ros::Time&, const ros::Duration&)> ros_callback) { +void FrankaHW::control( + franka::Robot& robot, + const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) { if (!controller_active_) { return; } @@ -195,7 +197,7 @@ bool FrankaHW::checkForConflict(const std::list<hardware_interface::ControllerIn uint8_t other_claims = 0; if (map_it->second.size() == 2) { for (auto& claimed_by : map_it->second) { - if (claimed_by[2].compare("hardware_interface::EffortJointInterface") == 0) { + if (claimed_by[2] == "hardware_interface::EffortJointInterface") { torque_claims++; } else { other_claims++;