diff --git a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h b/franka_example_controllers/include/franka_example_controllers/model_example_controller.h index c80ad74761784a53ef2f875b3c8058f31bc9f1e6..e1b4aef2dd18c45fe7388db300f6a5c5f78704fb 100644 --- a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/model_example_controller.h @@ -17,7 +17,8 @@ namespace franka_example_controllers { class ModelExampleController - : public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface> { + : public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface, + franka_hw::FrankaStateInterface> { public: bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; void update(const ros::Time&, const ros::Duration&) override; @@ -27,7 +28,6 @@ class ModelExampleController std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_; franka_hw::FrankaModelInterface* model_interface_; std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; - std::string arm_id_; franka_hw::TriggerRate rate_trigger_{1.0}; }; diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp index 1742520b717144ade02ab60330c0847c1f48df2c..dc165793f04fd88da27e835f421ba0cf8e1a80b0 100644 --- a/franka_example_controllers/src/model_example_controller.cpp +++ b/franka_example_controllers/src/model_example_controller.cpp @@ -31,12 +31,14 @@ namespace franka_example_controllers { bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { + std::string arm_id; + franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>(); if (franka_state_interface_ == nullptr) { ROS_ERROR("ModelExampleController: Could not get Franka state interface from hardware"); return false; } - if (!node_handle.getParam("arm_id", arm_id_)) { + if (!node_handle.getParam("arm_id", arm_id)) { ROS_ERROR("ModelExampleController: Could not read parameter arm_id"); return false; } @@ -48,7 +50,7 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw, try { franka_state_handle_.reset( - new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id_ + "_robot"))); + new 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,7 +59,7 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw, try { model_handle_.reset( - new franka_hw::FrankaModelHandle(model_interface_->getHandle(arm_id_ + "_model"))); + new 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());