9 #include <gtest/gtest.h> 26 std::string
arm_id(
"panda");
28 arm_id +
"_joint1",
arm_id +
"_joint2", arm_id +
"_joint3", arm_id +
"_joint4",
29 arm_id +
"_joint5", arm_id +
"_joint6", arm_id +
"_joint7"};
34 const std::string& name,
35 const std::string& type,
46 const std::string& name,
47 const std::string& type,
56 const std::string& name,
57 const std::string& type,
67 :
public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
72 EXPECT_TRUE(
robot_->initParameters(root_nh, private_nh));
73 robot_->initROSInterfaces(private_nh);
76 return robot_->checkForConflict(info_list);
79 return robot_->prepareSwitch(info_list, info_list);
87 :
public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> {
92 EXPECT_TRUE(
robot_->initParameters(root_nh, private_nh));
93 robot_->initROSInterfaces(private_nh);
94 robot_->setupParameterCallbacks(private_nh);
97 return robot_->checkForConflict(info_list);
100 return robot_->prepareSwitch(info_list, info_list);
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");
115 string type_str(
"SomeControllerClass");
117 joint_names[4], joint_names[5], joint_names[6]};
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}));
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}));
set< string > cartesian_arm2_set
ControllerInfo jt_cp_info
ControllerInfo jp_jp_info
std::unique_ptr< FrankaHW > robot_
string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface")
ControllerInfo jp_jp_jp_info
ControllerInfo unknown_iface_info
string jv_iface_str("hardware_interface::VelocityJointInterface")
ControllerInfo cp_arm2_info
string name_str("some_controller")
InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set)
InterfaceResources jp_res(jp_iface_str, joints_set)
ControllerInfo jp_cv_info
ControllerInfo jt_jv_info
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}))
InterfaceResources unknown_iface_res(unknown_iface_str, joints_set)
TEST_P(ControllerConflict, ConflictsForIncompatibleControllers)
string jp_iface_str("hardware_interface::PositionJointInterface")
ControllerInfo no_id_info
std::vector< InterfaceResources > claimed_resources
ControllerInfo jt_cv_info
string unknown_iface_str("hardware_interface::UnknownInterface")
bool callPrepareSwitch(const std::list< hardware_interface::ControllerInfo > info_list)
std::unique_ptr< FrankaHW > robot_
ControllerInfo jv_cv_info
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)
ControllerInfo cv_cv_info
bool callPrepareSwitch(const std::list< hardware_interface::ControllerInfo > info_list)
ControllerInfo jt_jp_info
ControllerInfo jp_jv_info
ControllerInfo jt_jt_info
ControllerInfo jv_cp_info
InterfaceResources jv_res(jv_iface_str, joints_set)
This class wraps the functionality of libfranka for controlling Panda robots into the ros_control fra...
ControllerInfo jp_cp_info
set< string > cartesian_set
string cp_iface_str("franka_hw::FrankaPoseCartesianInterface")
ControllerInfo cv_cp_info
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
ControllerInfo cp_cp_info
ControllerInfo jv_jv_info