From 1ea06624619305743b0be09a3f1f9bacdbb02fea Mon Sep 17 00:00:00 2001 From: Bo Chen Date: Thu, 4 Oct 2018 13:30:09 +0200 Subject: [PATCH] Add tests for hasError() method --- .../combined_robot_hw_tests/my_robot_hw_5.h | 54 ++++++++++++++++ combined_robot_hw_tests/src/my_robot_hw_5.cpp | 61 +++++++++++++++++++ combined_robot_hw_tests/test/cm_test.test | 2 + .../test/combined_robot_hw_test.cpp | 3 + .../test/combined_robot_hw_test.test | 3 + .../test_robot_hw_plugin.xml | 6 ++ 6 files changed, 129 insertions(+) create mode 100644 combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_5.h create mode 100644 combined_robot_hw_tests/src/my_robot_hw_5.cpp diff --git a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_5.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_5.h new file mode 100644 index 000000000..e9c8c733a --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_5.h @@ -0,0 +1,54 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + + + +#ifndef CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_5_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_5_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW5 : public hardware_interface::RobotHW +{ +public: + MyRobotHW5(); + virtual ~MyRobotHW5(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void read(const ros::Time& time, const ros::Duration& period); + void write(const ros::Time& time, const ros::Duration& period); + bool hasError(); + +}; +} + + +#endif diff --git a/combined_robot_hw_tests/src/my_robot_hw_5.cpp b/combined_robot_hw_tests/src/my_robot_hw_5.cpp new file mode 100644 index 000000000..d19595513 --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_5.cpp @@ -0,0 +1,61 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Shadow Robot Company Ltd. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + + +#include + +namespace combined_robot_hw_tests +{ + +MyRobotHW5::MyRobotHW5() +{ +} + +bool MyRobotHW5::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + return true; +} + + +void MyRobotHW5::read(const ros::Time& time, const ros::Duration& period) +{ + +} + +void MyRobotHW5::write(const ros::Time& time, const ros::Duration& period) +{ +} + +bool MyRobotHW5::hasError() +{ + return true; +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW5, hardware_interface::RobotHW) + diff --git a/combined_robot_hw_tests/test/cm_test.test b/combined_robot_hw_tests/test/cm_test.test index f36390ae5..8eb042c9a 100644 --- a/combined_robot_hw_tests/test/cm_test.test +++ b/combined_robot_hw_tests/test/cm_test.test @@ -20,6 +20,8 @@ joint_name_filter: right_arm my_robot_hw_4: type: combined_robot_hw_tests/MyRobotHW4 + my_robot_hw_5: + type: combined_robot_hw_tests/MyRobotHW5 my_controller: type: controller_manager_tests/EffortTestController my_controller2: diff --git a/combined_robot_hw_tests/test/combined_robot_hw_test.cpp b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp index 21e02e963..4d667c416 100644 --- a/combined_robot_hw_tests/test/combined_robot_hw_test.cpp +++ b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp @@ -94,6 +94,9 @@ TEST(CombinedRobotHWTests, combinationOk) robot_hw.write(ros::Time::now(), period); ej_handle = ej_interface->getHandle("test_joint2"); ASSERT_FLOAT_EQ(3.5, ej_handle.getCommand()); + + bool error = robot_hw.hasError(); + ASSERT_TRUE(error); } TEST(CombinedRobotHWTests, switchOk) diff --git a/combined_robot_hw_tests/test/combined_robot_hw_test.test b/combined_robot_hw_tests/test/combined_robot_hw_test.test index b963f2548..46a3ac14a 100644 --- a/combined_robot_hw_tests/test/combined_robot_hw_test.test +++ b/combined_robot_hw_tests/test/combined_robot_hw_test.test @@ -5,6 +5,7 @@ - my_robot_hw_2 - my_robot_hw_3 - my_robot_hw_4 + - my_robot_hw_5 my_robot_hw_1: type: combined_robot_hw_tests/MyRobotHW1 my_robot_hw_2: @@ -18,6 +19,8 @@ joint_name_filter: right_arm my_robot_hw_4: type: combined_robot_hw_tests/MyRobotHW4 + my_robot_hw_5: + type: combined_robot_hw_tests/MyRobotHW5 diff --git a/combined_robot_hw_tests/test_robot_hw_plugin.xml b/combined_robot_hw_tests/test_robot_hw_plugin.xml index 43384c87b..cf6b8e1a5 100644 --- a/combined_robot_hw_tests/test_robot_hw_plugin.xml +++ b/combined_robot_hw_tests/test_robot_hw_plugin.xml @@ -22,4 +22,10 @@ A type of RobotHW + + + + A type of RobotHW + +