From d98a9b12869b760d458000ffb19fcf48f692f0d8 Mon Sep 17 00:00:00 2001
From: Simon Gabl <simon.gabl@franka.de>
Date: Wed, 24 Jan 2018 15:43:41 +0100
Subject: [PATCH] Update Model Example Controller.
---
.../franka_example_controllers/model_example_controller.h | 4 ++--
.../src/model_example_controller.cpp | 8 +++++---
2 files changed, 7 insertions(+), 5 deletions(-)
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 c80ad74..e1b4aef 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 1742520..dc16579 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());
--
GitLab