franka_hw_controller_switching_test.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #include <array>
4 #include <list>
5 #include <random>
6 #include <set>
7 #include <string>
8 
9 #include <gtest/gtest.h>
10 
15 #include <ros/ros.h>
16 #include <urdf/model.h>
17 
19 #include <franka_hw/franka_hw.h>
20 
21 using std::string;
22 using std::set;
25 
26 std::string arm_id("panda");
27 std::array<std::string, 7> joint_names = {
28  arm_id + "_joint1", arm_id + "_joint2", arm_id + "_joint3", arm_id + "_joint4",
29  arm_id + "_joint5", arm_id + "_joint6", arm_id + "_joint7"};
30 
31 namespace franka_hw {
32 
34  const std::string& name,
35  const std::string& type,
36  const hardware_interface::InterfaceResources& resource1) {
38  info.claimed_resources.clear();
39  info.name = name;
40  info.type = type;
41  info.claimed_resources.push_back(resource1);
42  return info;
43 }
44 
46  const std::string& name,
47  const std::string& type,
49  const hardware_interface::InterfaceResources& resource2) {
50  hardware_interface::ControllerInfo info = newInfo(name, type, resource1);
51  info.claimed_resources.push_back(resource2);
52  return info;
53 }
54 
56  const std::string& name,
57  const std::string& type,
60  const hardware_interface::InterfaceResources& resource3) {
61  hardware_interface::ControllerInfo info = newInfo(name, type, resource1, resource2);
62  info.claimed_resources.push_back(resource3);
63  return info;
64 }
65 
67  : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
68  public:
69  ControllerConflict() : robot_(std::make_unique<FrankaHW>()) {
70  ros::NodeHandle private_nh("~");
71  ros::NodeHandle root_nh;
72  EXPECT_TRUE(robot_->initParameters(root_nh, private_nh));
73  robot_->initROSInterfaces(private_nh);
74  }
75  bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
76  return robot_->checkForConflict(info_list);
77  }
78  bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
79  return robot_->prepareSwitch(info_list, info_list);
80  }
81 
82  private:
83  std::unique_ptr<FrankaHW> robot_;
84 };
85 
87  : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
88  public:
89  NoControllerConflict() : robot_(std::make_unique<FrankaHW>()) {
90  ros::NodeHandle private_nh("~");
91  ros::NodeHandle root_nh;
92  EXPECT_TRUE(robot_->initParameters(root_nh, private_nh));
93  robot_->initROSInterfaces(private_nh);
94  robot_->setupParameterCallbacks(private_nh);
95  }
96  bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
97  return robot_->checkForConflict(info_list);
98  }
99  bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) {
100  return robot_->prepareSwitch(info_list, info_list);
101  }
102 
103  private:
104  std::unique_ptr<FrankaHW> robot_;
105 };
106 
107 string arm_id2("panda2");
108 string jp_iface_str("hardware_interface::PositionJointInterface");
109 string jv_iface_str("hardware_interface::VelocityJointInterface");
110 string jt_iface_str("hardware_interface::EffortJointInterface");
111 string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface");
112 string cp_iface_str("franka_hw::FrankaPoseCartesianInterface");
113 string unknown_iface_str("hardware_interface::UnknownInterface");
114 string name_str("some_controller");
115 string type_str("SomeControllerClass");
116 set<string> joints_set = {joint_names[0], joint_names[1], joint_names[2], joint_names[3],
117  joint_names[4], joint_names[5], joint_names[6]};
118 set<string> cartesian_set = {arm_id + "_robot"};
119 set<string> cartesian_arm2_set = {arm_id2 + "_robot"};
120 set<string> no_id_set = {"joint1"};
126 InterfaceResources cv_res(cv_iface_str, cartesian_set);
127 InterfaceResources cp_res(cp_iface_str, cartesian_set);
128 InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set);
153 
154 INSTANTIATE_TEST_CASE_P(nonAdmissibleRequests,
156  ::testing::Values(std::list<ControllerInfo>{no_id_info},
157  std::list<ControllerInfo>{unknown_iface_info},
158  std::list<ControllerInfo>{jt_jt_info},
159  std::list<ControllerInfo>{jp_jp_info},
160  std::list<ControllerInfo>{jp_jv_info},
161  std::list<ControllerInfo>{jp_cv_info},
162  std::list<ControllerInfo>{jp_cp_info},
163  std::list<ControllerInfo>{jv_jv_info},
164  std::list<ControllerInfo>{jv_cv_info},
165  std::list<ControllerInfo>{jv_cp_info},
166  std::list<ControllerInfo>{cv_cv_info},
167  std::list<ControllerInfo>{cv_cp_info},
168  std::list<ControllerInfo>{cp_cp_info},
169  std::list<ControllerInfo>{jp_jp_jp_info}));
170 
171 INSTANTIATE_TEST_CASE_P(admissibleRequests,
173  ::testing::Values(std::list<ControllerInfo>{jp_info},
174  std::list<ControllerInfo>{jv_info},
175  std::list<ControllerInfo>{jt_info},
176  std::list<ControllerInfo>{cv_info},
177  std::list<ControllerInfo>{cp_info},
178  std::list<ControllerInfo>{jt_jp_info},
179  std::list<ControllerInfo>{jt_jv_info},
180  std::list<ControllerInfo>{jt_cv_info},
181  std::list<ControllerInfo>{jt_cp_info},
182  std::list<ControllerInfo>{cp_info, cp_arm2_info}));
183 
184 TEST_P(ControllerConflict, ConflictsForIncompatibleControllers) {
185  EXPECT_TRUE(callCheckForConflict(GetParam()));
186 }
187 
188 TEST_P(NoControllerConflict, DoesNotConflictForCompatibleControllers) {
189  EXPECT_FALSE(callCheckForConflict(GetParam()));
190 }
191 
192 TEST_P(NoControllerConflict, CanPrepareSwitchForCompatibleControllers) {
193  EXPECT_TRUE(callPrepareSwitch(GetParam()));
194 }
195 
196 } // namespace franka_hw
string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface")
string jv_iface_str("hardware_interface::VelocityJointInterface")
string name_str("some_controller")
InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set)
InterfaceResources jp_res(jp_iface_str, joints_set)
INSTANTIATE_TEST_CASE_P(nonAdmissibleRequests, ControllerConflict,::testing::Values(std::list< ControllerInfo >{no_id_info}, std::list< ControllerInfo >{unknown_iface_info}, std::list< ControllerInfo >{jt_jt_info}, std::list< ControllerInfo >{jp_jp_info}, std::list< ControllerInfo >{jp_jv_info}, std::list< ControllerInfo >{jp_cv_info}, std::list< ControllerInfo >{jp_cp_info}, std::list< ControllerInfo >{jv_jv_info}, std::list< ControllerInfo >{jv_cv_info}, std::list< ControllerInfo >{jv_cp_info}, std::list< ControllerInfo >{cv_cv_info}, std::list< ControllerInfo >{cv_cp_info}, std::list< ControllerInfo >{cp_cp_info}, std::list< ControllerInfo >{jp_jp_jp_info}))
string arm_id2("panda2")
InterfaceResources unknown_iface_res(unknown_iface_str, joints_set)
TEST_P(ControllerConflict, ConflictsForIncompatibleControllers)
string jp_iface_str("hardware_interface::PositionJointInterface")
std::vector< InterfaceResources > claimed_resources
string unknown_iface_str("hardware_interface::UnknownInterface")
bool callPrepareSwitch(const std::list< hardware_interface::ControllerInfo > info_list)
bool callCheckForConflict(const std::list< hardware_interface::ControllerInfo > info_list)
InterfaceResources no_id_res(jp_iface_str, no_id_set)
hardware_interface::ControllerInfo newInfo(const std::string &name, const std::string &type, const hardware_interface::InterfaceResources &resource1)
std::string arm_id("panda")
InterfaceResources cv_res(cv_iface_str, cartesian_set)
bool callCheckForConflict(const std::list< hardware_interface::ControllerInfo > info_list)
bool callPrepareSwitch(const std::list< hardware_interface::ControllerInfo > info_list)
InterfaceResources jv_res(jv_iface_str, joints_set)
This class wraps the functionality of libfranka for controlling Panda robots into the ros_control fra...
Definition: franka_hw.h:38
string cp_iface_str("franka_hw::FrankaPoseCartesianInterface")
InterfaceResources cp_res(cp_iface_str, cartesian_set)
string jt_iface_str("hardware_interface::EffortJointInterface")
InterfaceResources jt_res(jt_iface_str, joints_set)
string type_str("SomeControllerClass")
std::array< std::string, 7 > joint_names


franka_hw
Author(s): Franka Emika GmbH
autogenerated on Fri Oct 23 2020 03:47:05