diff --git a/combined_robot_hw/CMakeLists.txt b/combined_robot_hw/CMakeLists.txt new file mode 100644 index 000000000..9a76b2dc1 --- /dev/null +++ b/combined_robot_hw/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8.3) +project(combined_robot_hw) + +find_package(catkin REQUIRED COMPONENTS + hardware_interface + pluginlib + roscpp +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS hardware_interface pluginlib roscpp +) + +add_library(${PROJECT_NAME} + src/combined_robot_hw.cpp +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + + +# Install +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + diff --git a/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h b/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h new file mode 100644 index 000000000..df29d56d7 --- /dev/null +++ b/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h @@ -0,0 +1,115 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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. +////////////////////////////////////////////////////////////////////////////// + +#ifndef COMBINED_ROBOT_HARDWARE_COMBINED_ROBOT_HARDWARE_H +#define COMBINED_ROBOT_HARDWARE_COMBINED_ROBOT_HARDWARE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace combined_robot_hardware +{ + +/** \brief CombinedRobotHW + * + * This class provides a way to combine RobotHW objects. + * + * + * + */ +class CombinedRobotHW : public hardware_interface::RobotHW +{ +public: + CombinedRobotHW(); + + virtual ~CombinedRobotHW(){} + + /** \brief The init function is called to initialize the RobotHW from a + * non-realtime thread. + * + * \param root_nh A NodeHandle in the root of the caller namespace. + * + * \param robot_hw_nh A NodeHandle in the namespace from which the RobotHW + * should read its configuration. + * + * \returns True if initialization was successful + */ + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + + + /** + * Check (in non-realtime) if given controllers could be started and stopped from the current state of the RobotHW + * with regard to necessary hardware interface switches and prepare the switching. Start and stop list are disjoint. + * This handles the check and preparation, the actual switch is commited in doSwitch() + */ + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + + /** + * Perform (in realtime) all necessary hardware interface switches in order to start and stop the given controllers. + * Start and stop list are disjoint. The feasability was checked in prepareSwitch() beforehand. + */ + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + + /** + * Reads data from the robot HW + */ + virtual void read(); + + /** + * Writes data to the robot HW + */ + virtual void write(); + +protected: + ros::NodeHandle root_nh_; + ros::NodeHandle robot_hw_nh_; + pluginlib::ClassLoader robot_hw_loader_; + std::vector > robot_hw_list_; + + virtual bool loadRobotHW(const std::string& name); + + /** \brief Filters the start and stop lists so that they only contain the controllers and + * resources that correspond to the r_hw interface manager + */ + void filterControllerList(const std::list& list, + std::list& filtered_list, + boost::shared_ptr r_hw); +}; + +} + +#endif diff --git a/combined_robot_hw/package.xml b/combined_robot_hw/package.xml new file mode 100644 index 000000000..8f7c503a9 --- /dev/null +++ b/combined_robot_hw/package.xml @@ -0,0 +1,27 @@ + + + combined_robot_hw + 0.0.0 + Combined Robot HW class. + Toni Oliver + + BSD + + https://github.com/ros-controls/ros_control/wiki + https://github.com/ros-controls/ros_control/issues + https://github.com/ros-controls/ros_control + + Toni Oliver + + catkin + hardware_interface + pluginlib + roscpp + hardware_interface + pluginlib + roscpp + + + + + \ No newline at end of file diff --git a/combined_robot_hw/src/combined_robot_hw.cpp b/combined_robot_hw/src/combined_robot_hw.cpp new file mode 100644 index 000000000..431752433 --- /dev/null +++ b/combined_robot_hw/src/combined_robot_hw.cpp @@ -0,0 +1,238 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 +#include "combined_robot_hw/combined_robot_hw.h" + +namespace combined_robot_hardware +{ + CombinedRobotHW::CombinedRobotHW() : + robot_hw_loader_("hardware_interface", "hardware_interface::RobotHW") + {} + + bool CombinedRobotHW::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) + { + root_nh_ = root_nh; + robot_hw_nh_ = robot_hw_nh; + + std::vector robots; + if (!robot_hw_nh.getParam("robot_hardware", robots)) {return false;} + + std::vector::iterator it; + for (it = robots.begin(); it != robots.end(); it++) + { + if (!loadRobotHW(*it)) + { + return false; + } + } + return true; + } + + bool CombinedRobotHW::prepareSwitch(const std::list& start_list, + const std::list& stop_list) + { + // Call the prepareSwitch method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + std::list filtered_start_list; + std::list filtered_stop_list; + + // Generate a filtered version of start_list and stop_list for each RobotHW before calling prepareSwitch + filterControllerList(start_list, filtered_start_list, *robot_hw); + filterControllerList(stop_list, filtered_stop_list, *robot_hw); + + if (!(*robot_hw)->prepareSwitch(filtered_start_list, filtered_stop_list)) + return false; + } + return true; + } + + void CombinedRobotHW::doSwitch(const std::list& start_list, + const std::list& stop_list) + { + // Call the doSwitch method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + std::list filtered_start_list; + std::list filtered_stop_list; + + // Generate a filtered version of start_list and stop_list for each RobotHW before calling doSwitch + filterControllerList(start_list, filtered_start_list, *robot_hw); + filterControllerList(stop_list, filtered_stop_list, *robot_hw); + + (*robot_hw)->doSwitch(filtered_start_list, filtered_stop_list); + } + } + + bool CombinedRobotHW::loadRobotHW(const std::string& name) + { + ROS_DEBUG("Will load robot HW '%s'", name.c_str()); + + ros::NodeHandle c_nh; + // Constructs the robot HW + try{ + c_nh = ros::NodeHandle(robot_hw_nh_, name); + } + catch(std::exception &e) { + ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s':\n%s", name.c_str(), e.what()); + return false; + } + catch(...){ + ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s'", name.c_str()); + return false; + } + boost::shared_ptr r_hw; + std::string type; + if (c_nh.getParam("type", type)) + { + ROS_DEBUG("Constructing robot HW '%s' of type '%s'", name.c_str(), type.c_str()); + try + { + std::vector cur_types = robot_hw_loader_.getDeclaredClasses(); + for(size_t i=0; i < cur_types.size(); i++){ + if (type == cur_types[i]){ + r_hw = robot_hw_loader_.createInstance(type); + } + } + } + catch (const std::runtime_error &ex) + { + ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what()); + } + } + else + { + ROS_ERROR("Could not load robot HW '%s' because the type was not specified. Did you load the robot HW configuration on the parameter server (namespace: '%s')?", name.c_str(), c_nh.getNamespace().c_str()); + return false; + } + + // checks if robot HW was constructed + if (!r_hw) + { + ROS_ERROR("Could not load robot HW '%s' because robot HW type '%s' does not exist.", name.c_str(), type.c_str()); + return false; + } + + // Initializes the robot HW + ROS_DEBUG("Initializing robot HW '%s'", name.c_str()); + bool initialized; + try{ + initialized = r_hw->init(root_nh_, c_nh); + } + catch(std::exception &e){ + ROS_ERROR("Exception thrown while initializing robot HW %s.\n%s", name.c_str(), e.what()); + initialized = false; + } + catch(...){ + ROS_ERROR("Exception thrown while initializing robot HW %s", name.c_str()); + initialized = false; + } + + if (!initialized) + { + ROS_ERROR("Initializing robot HW '%s' failed", name.c_str()); + return false; + } + ROS_DEBUG("Initialized robot HW '%s' successful", name.c_str()); + + robot_hw_list_.push_back(r_hw); + + this->registerInterfaceManager(r_hw.get()); + + ROS_DEBUG("Successfully load robot HW '%s'", name.c_str()); + return true; + } + + void CombinedRobotHW::read() + { + // Call the read method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + (*robot_hw)->read(); + } + } + + + void CombinedRobotHW::write() + { + // Call the write method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + (*robot_hw)->write(); + } + } + + void CombinedRobotHW::filterControllerList(const std::list& list, + std::list& filtered_list, + boost::shared_ptr r_hw) + { + filtered_list.clear(); + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) + { + hardware_interface::ControllerInfo filtered_controller; + filtered_controller.name = it->name; + filtered_controller.type = it->type; + + if (it->claimed_resources.empty()) + { + filtered_list.push_back(filtered_controller); + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + hardware_interface::InterfaceResources filtered_iface_resources; + filtered_iface_resources.hardware_interface = res_it->hardware_interface; + std::vector r_hw_ifaces = r_hw->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), filtered_iface_resources.hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered in r_hw, so we filter it out + { + continue; + } + + std::vector r_hw_iface_resources = r_hw->getInterfaceResources(filtered_iface_resources.hardware_interface); + std::set filtered_resources; + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name != r_hw_iface_resources.end()) + { + filtered_resources.insert(*ctrl_res); + } + } + filtered_iface_resources.resources = filtered_resources; + filtered_controller.claimed_resources.push_back(filtered_iface_resources); + } + filtered_list.push_back(filtered_controller); + } + } +} diff --git a/combined_robot_hw_tests/CMakeLists.txt b/combined_robot_hw_tests/CMakeLists.txt new file mode 100644 index 000000000..1d088859b --- /dev/null +++ b/combined_robot_hw_tests/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 2.8.3) +project(combined_robot_hw_tests) + +find_package(catkin REQUIRED COMPONENTS + combined_robot_hw + controller_manager + controller_manager_tests + hardware_interface + roscpp +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS combined_robot_hw hardware_interface roscpp +) + +add_library(${PROJECT_NAME} + src/my_robot_hw_1.cpp + src/my_robot_hw_2.cpp + src/my_robot_hw_3.cpp + src/my_robot_hw_4.cpp +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable(combined_robot_hw_dummy_app src/dummy_app.cpp) +target_link_libraries(combined_robot_hw_dummy_app ${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + + find_package(rostest REQUIRED) + add_rostest_gtest(combined_robot_hw_test + test/combined_robot_hw_test.test + test/combined_robot_hw_test.cpp + ) + target_link_libraries(combined_robot_hw_test ${PROJECT_NAME} ${catkin_LIBRARIES}) + + add_rostest_gtest(combined_robot_hw_cm_test + test/cm_test.test + test/cm_test.cpp + ) + target_link_libraries(combined_robot_hw_cm_test ${PROJECT_NAME} ${catkin_LIBRARIES}) + +endif() + +# Install +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(FILES test_robot_hw_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h new file mode 100644 index 000000000..ad106166f --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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_1_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_1_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW1 : public hardware_interface::RobotHW +{ +public: + MyRobotHW1(); + virtual ~MyRobotHW1(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + virtual void read(); + virtual void write(); + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + +protected: + +private: + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + std::vector joint_effort_command_; + std::vector joint_velocity_command_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_name_; +}; +} + + +#endif diff --git a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_2.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_2.h new file mode 100644 index 000000000..004710411 --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_2.h @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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_2_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_2_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW2 : public hardware_interface::RobotHW +{ +public: + MyRobotHW2(); + virtual ~MyRobotHW2(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void read(); + void write(); + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + +protected: + +private: + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + std::vector joint_effort_command_; + std::vector joint_velocity_command_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_name_; +}; +} + + +#endif diff --git a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_3.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_3.h new file mode 100644 index 000000000..a2a37ed1a --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_3.h @@ -0,0 +1,66 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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_3_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_3_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW3 : public hardware_interface::RobotHW +{ +public: + MyRobotHW3(); + virtual ~MyRobotHW3(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void read(); + void write(); + +protected: + +private: + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + std::vector joint_effort_command_; + std::vector joint_velocity_command_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_name_; +}; +} + + +#endif diff --git a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_4.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_4.h new file mode 100644 index 000000000..39cea4a97 --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_4.h @@ -0,0 +1,67 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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_4_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_4_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW4 : public hardware_interface::RobotHW +{ +public: + MyRobotHW4(); + virtual ~MyRobotHW4(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void read(); + void write(); + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + +protected: + +private: + hardware_interface::ForceTorqueSensorInterface ft_sensor_interface_; + hardware_interface::HardwareInterface a_plain_hw_interface_; + + double force_[3]; + double torque_[3]; + std::string sensor_name_; + std::string frame_id_; +}; +} + + +#endif diff --git a/combined_robot_hw_tests/package.xml b/combined_robot_hw_tests/package.xml new file mode 100644 index 000000000..d9693aa52 --- /dev/null +++ b/combined_robot_hw_tests/package.xml @@ -0,0 +1,33 @@ + + + combined_robot_hw_tests + 0.0.0 + The combined_robot_hw_tests package + Toni Oliver + + BSD + + https://github.com/ros-controls/ros_control/wiki + https://github.com/ros-controls/ros_control/issues + https://github.com/ros-controls/ros_control + + Toni Oliver + + catkin + combined_robot_hw + controller_manager + controller_manager_tests + hardware_interface + roscpp + rostest + combined_robot_hw + controller_manager + controller_manager_tests + hardware_interface + roscpp + rostest + + + + + \ No newline at end of file diff --git a/combined_robot_hw_tests/src/dummy_app.cpp b/combined_robot_hw_tests/src/dummy_app.cpp new file mode 100644 index 000000000..a943b914f --- /dev/null +++ b/combined_robot_hw_tests/src/dummy_app.cpp @@ -0,0 +1,55 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, 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 +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "DummyApp"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + ros::NodeHandle nh; + combined_robot_hardware::CombinedRobotHW hw; + bool init_success = hw.init(nh, nh); + + controller_manager::ControllerManager cm(&hw, nh); + + ros::Duration period(1.0); + while (ros::ok()) + { + ROS_INFO("loop"); + hw.read(); + cm.update(ros::Time::now(), period); + hw.write(); + period.sleep(); + } +} + diff --git a/combined_robot_hw_tests/src/my_robot_hw_1.cpp b/combined_robot_hw_tests/src/my_robot_hw_1.cpp new file mode 100644 index 000000000..b337aefc8 --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_1.cpp @@ -0,0 +1,175 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 +#include + +namespace combined_robot_hw_tests +{ + +MyRobotHW1::MyRobotHW1() +{ +} + +bool MyRobotHW1::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + // Initialize raw data + joint_position_.resize(3); + joint_velocity_.resize(3); + joint_effort_.resize(3); + joint_effort_command_.resize(3); + joint_velocity_command_.resize(3); + joint_name_.resize(3); + + joint_name_[0] = "test_joint1"; + joint_position_[0] = 1.0; + joint_velocity_[0] = 0.0; + joint_effort_[0] = 0.1; + joint_effort_command_[0] = 3.0; + joint_velocity_command_[0] = 0.0; + + joint_name_[1] = "test_joint2"; + joint_position_[1] = 1.0; + joint_velocity_[1] = 0.0; + joint_effort_[1] = 0.1; + joint_effort_command_[1] = 0.0; + joint_velocity_command_[1] = 0.0; + + joint_name_[2] = "test_joint3"; + joint_position_[2] = 1.0; + joint_velocity_[2] = 0.0; + joint_effort_[2] = 0.1; + joint_effort_command_[2] = 0.0; + joint_velocity_command_[2] = 0.0; + + // Populate hardware interfaces + js_interface_.registerHandle(JointStateHandle(joint_name_[0], &joint_position_[0], &joint_velocity_[0], &joint_effort_[0])); + js_interface_.registerHandle(JointStateHandle(joint_name_[1], &joint_position_[1], &joint_velocity_[1], &joint_effort_[1])); + js_interface_.registerHandle(JointStateHandle(joint_name_[2], &joint_position_[2], &joint_velocity_[2], &joint_effort_[2])); + + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[0]), &joint_effort_command_[0])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[1]), &joint_effort_command_[1])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[2]), &joint_effort_command_[2])); + + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[0]), &joint_velocity_command_[0])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[1]), &joint_velocity_command_[1])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[2]), &joint_velocity_command_[2])); + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&vj_interface_); + + return true; +} + + +void MyRobotHW1::read() +{ + joint_position_[0] = 2.7; +} + +void MyRobotHW1::write() +{ + // Just to test that write() is called + joint_effort_command_[1] = joint_effort_command_[0]; +} + +bool MyRobotHW1::prepareSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad interface: " << res_it->hardware_interface); + std::cout << res_it->hardware_interface; + return false; + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad resource: " << (*ctrl_res)); + std::cout << (*ctrl_res); + return false; + } + } + } + } + return true; +} + +void MyRobotHW1::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Hardware_interface " + res_it->hardware_interface + " is not registered"); + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Resource " + *ctrl_res + " is not registered"); + } + } + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW1, hardware_interface::RobotHW) diff --git a/combined_robot_hw_tests/src/my_robot_hw_2.cpp b/combined_robot_hw_tests/src/my_robot_hw_2.cpp new file mode 100644 index 000000000..08aec7270 --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_2.cpp @@ -0,0 +1,155 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 +#include + +namespace combined_robot_hw_tests +{ + +MyRobotHW2::MyRobotHW2() +{ +} + +bool MyRobotHW2::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + std::vector joints; + if (!robot_hw_nh.getParam("joints", joints)) {return false;} + + // Initialize raw data + size_t nb_joints = joints.size(); + joint_position_.resize(nb_joints); + joint_velocity_.resize(nb_joints); + joint_effort_.resize(nb_joints); + joint_effort_command_.resize(nb_joints); + joint_velocity_command_.resize(nb_joints); + joint_name_.resize(nb_joints); + + for (size_t i = 0; i < nb_joints; i++) + { + joint_name_[i] = joints[i]; + joint_position_[i] = 1.0; + joint_velocity_[i] = 0.0; + joint_effort_[i] = 0.1; + joint_effort_command_[i] = 0.0; + joint_velocity_command_[i] = 0.0; + // Populate hardware interfaces + js_interface_.registerHandle(JointStateHandle(joint_name_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_effort_command_[i])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_velocity_command_[i])); + } + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&vj_interface_); + + return true; +} + + +void MyRobotHW2::read() +{ + +} + +void MyRobotHW2::write() +{ +} + +bool MyRobotHW2::prepareSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad interface: " << res_it->hardware_interface); + return false; + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad resource: " << (*ctrl_res)); + return false; + } + } + } + } + return true; +} + +void MyRobotHW2::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Hardware_interface " + res_it->hardware_interface + " is not registered"); + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Resource " + *ctrl_res + " is not registered"); + } + } + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW2, hardware_interface::RobotHW) diff --git a/combined_robot_hw_tests/src/my_robot_hw_3.cpp b/combined_robot_hw_tests/src/my_robot_hw_3.cpp new file mode 100644 index 000000000..d6f576060 --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_3.cpp @@ -0,0 +1,94 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 +{ + +MyRobotHW3::MyRobotHW3() +{ +} + +bool MyRobotHW3::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + std::string robot_description; + if (!robot_hw_nh.getParam("robot_description", robot_description)) {return false;} + + // E.g. Read URDF and parse joint names. + // Then read joint_name_filter param to know what joints belong to this RobotHW + + std::vector joints; + joints.push_back("right_arm_joint_1"); + + // Initialize raw data + size_t nb_joints = joints.size(); + joint_position_.resize(nb_joints); + joint_velocity_.resize(nb_joints); + joint_effort_.resize(nb_joints); + joint_effort_command_.resize(nb_joints); + joint_velocity_command_.resize(nb_joints); + joint_name_.resize(nb_joints); + + for (size_t i = 0; i < nb_joints; i++) + { + joint_name_[i] = joints[i]; + joint_position_[i] = 1.0; + joint_velocity_[i] = 0.0; + joint_effort_[i] = 0.1; + joint_effort_command_[i] = 1.5; + joint_velocity_command_[i] = 0.0; + // Populate hardware interfaces + js_interface_.registerHandle(JointStateHandle(joint_name_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_effort_command_[i])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_velocity_command_[i])); + } + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&vj_interface_); + + return true; +} + + +void MyRobotHW3::read() +{ + +} + +void MyRobotHW3::write() +{ +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW3, hardware_interface::RobotHW) + diff --git a/combined_robot_hw_tests/src/my_robot_hw_4.cpp b/combined_robot_hw_tests/src/my_robot_hw_4.cpp new file mode 100644 index 000000000..847027e1b --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_4.cpp @@ -0,0 +1,113 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 +{ + +MyRobotHW4::MyRobotHW4() +{ +} + +bool MyRobotHW4::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + force_[0] = 0.0; + force_[1] = 0.1; + force_[2] = 0.2; + torque_[0] = 0.0; + torque_[1] = 0.1; + torque_[2] = 0.2; + sensor_name_ = "ft_sensor_1"; + frame_id_ = "link_1"; + + ft_sensor_interface_.registerHandle(ForceTorqueSensorHandle(sensor_name_, frame_id_, force_, torque_)); + + registerInterface(&ft_sensor_interface_); + registerInterface(&a_plain_hw_interface_); + + return true; +} + + +void MyRobotHW4::read() +{ + force_[2] = 1.2; +} + +void MyRobotHW4::write() +{ +} + +bool MyRobotHW4::prepareSwitch(const std::list& start_list, + const std::list& stop_list) +{ + // To easily test a failure case, any controller that claims resources on MyRobotHW4 will fail + if (!start_list.empty() || !stop_list.empty()) + { + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + else + { + return false; + } + } + } + return true; +} + +void MyRobotHW4::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + // To easily test a failure case, any controller that claims resources on MyRobotHW4 will fail + if (!start_list.empty() || !stop_list.empty()) + { + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + else + { + throw hardware_interface::HardwareInterfaceException("MyRobotHW4 can't switch controllers"); + } + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW4, hardware_interface::RobotHW) + diff --git a/combined_robot_hw_tests/test/cm_test.cpp b/combined_robot_hw_tests/test/cm_test.cpp new file mode 100644 index 000000000..025854d4d --- /dev/null +++ b/combined_robot_hw_tests/test/cm_test.cpp @@ -0,0 +1,521 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Adolfo Rodríguez Tsouroukdissian +/// \author Vijay Pradeep +/// \author Toni Oliver + +#include +#include + +#include +#include +#include +#include +#include + + +using namespace controller_manager_msgs; + +TEST(CMTests, loadUnloadOk) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + + // Load single-interface controller + { + LoadController srv; + srv.request.name = "my_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Load multi-interface controller: + // Two required interfaces + { + LoadController srv; + srv.request.name = "vel_eff_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Load multi-interface controller: + // One required (and existing) interface and one optional (and non-existent) interface + { + LoadController srv; + srv.request.name = "optional_interfaces_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload single-interface controller + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload multi-interface controllers + { + UnloadController srv; + srv.request.name = "vel_eff_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + { + UnloadController srv; + srv.request.name = "optional_interfaces_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } +} + +TEST(CMTests, loadUnloadKo) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + + // Load non-existent controller + { + LoadController srv; + srv.request.name = "nonexistent_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Load controller requesting non-existient HW interface + { + LoadController srv; + srv.request.name = "non_existent_interface_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Load controller requesting non-existent resource from valid HW interface + { + LoadController srv; + srv.request.name = "non_existent_resource_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Load multi-interface controller: + // Two required HW interfaces, of which one is non-existent + { + LoadController srv; + srv.request.name = "non_existing_multi_interface_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unload not loaded controller + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } +} + +TEST(CMTests, switchController) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + ros::ServiceClient switch_client = nh.serviceClient("/controller_manager/switch_controller"); + + // Load controllers + { + LoadController srv; + srv.request.name = "my_controller"; + load_client.call(srv); + srv.request.name = "my_controller2"; + load_client.call(srv); + srv.request.name = "vel_eff_controller"; + load_client.call(srv); + srv.request.name = "self_conflict_controller"; + load_client.call(srv); + } + + // Successful STRICT start + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful STRICT stop + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful STRICT start + { + SwitchController srv; + srv.request.start_controllers.push_back("vel_eff_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful STRICT stop+start + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.stop_controllers.push_back("vel_eff_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Back to no running controllers + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unsuccessful STRICT start + { + SwitchController srv; + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT stop + { + SwitchController srv; + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT switch: invalid stop + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT switch: invalid start + { + SwitchController srv; + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT start: Resource conflict within single controller + { + SwitchController srv; + srv.request.start_controllers.push_back("self_conflict_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT start: Resource conflict between two controllers + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.start_controllers.push_back("my_controller2"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Successful BEST_EFFORT switch: No-op + { + SwitchController srv; + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.BEST_EFFORT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful BEST_EFFORT switch: Partial success, only one started controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller2"); + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.BEST_EFFORT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful BEST_EFFORT switch: Partial success, one started and one stopped controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("my_controller2"); + srv.request.strictness = srv.request.BEST_EFFORT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Back to no running controllers + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload controllers + { + UnloadController srv; + srv.request.name = "my_controller"; + unload_client.call(srv); + srv.request.name = "my_controller2"; + unload_client.call(srv); + srv.request.name = "vel_eff_controller"; + unload_client.call(srv); + srv.request.name = "self_conflict_controller"; + unload_client.call(srv); + } +} + +TEST(CMTests, stopBeforeUnload) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + ros::ServiceClient switch_client = nh.serviceClient("/controller_manager/switch_controller"); + + // Load controller + { + LoadController srv; + srv.request.name = "my_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Start controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Try to unload running controller and fail + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Stop controller + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload stopped controller + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } +} + +TEST(CMTests, listControllerTypes) +{ + ros::NodeHandle nh; + ros::ServiceClient types_client = nh.serviceClient("/controller_manager/list_controller_types"); + + ListControllerTypes srv; + bool call_success = types_client.call(srv); + ASSERT_TRUE(call_success); + // Weak test that the number of available types and base classes is not lower than those defined in this test package + EXPECT_GE(srv.response.types.size(), 3); + EXPECT_GE(srv.response.base_classes.size(), 3); +} + +TEST(CMTests, listControllers) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + ros::ServiceClient switch_client = nh.serviceClient("/controller_manager/switch_controller"); + ros::ServiceClient list_client = nh.serviceClient("/controller_manager/list_controllers"); + + // Load controllers + { + LoadController srv; + srv.request.name = "my_controller"; + load_client.call(srv); + srv.request.name = "vel_eff_controller"; + load_client.call(srv); + } + + // Start one controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + switch_client.call(srv); + } + + // List controllers + { + ListControllers srv; + bool call_success = list_client.call(srv); + ASSERT_TRUE(call_success); + ASSERT_EQ(srv.response.controller.size(), 2); + + ControllerState state1, state2; + if (srv.response.controller[0].name == "my_controller") + { + state1 = srv.response.controller[0]; + state2 = srv.response.controller[1]; + } + else + { + state1 = srv.response.controller[1]; + state2 = srv.response.controller[0]; + } + + EXPECT_EQ(state1.name, "my_controller"); + EXPECT_EQ(state1.state, "running"); + EXPECT_EQ(state1.type, "controller_manager_tests/EffortTestController"); + ASSERT_EQ(state1.claimed_resources.size(), 1); + EXPECT_EQ(state1.claimed_resources[0].hardware_interface, "hardware_interface::EffortJointInterface"); + ASSERT_EQ(state1.claimed_resources[0].resources.size(), 2); + EXPECT_EQ(state1.claimed_resources[0].resources[0], "hiDOF_joint1"); + EXPECT_EQ(state1.claimed_resources[0].resources[1], "hiDOF_joint2"); + + EXPECT_EQ(state2.name, "vel_eff_controller"); + EXPECT_EQ(state2.state, "stopped"); + EXPECT_EQ(state2.type, "controller_manager_tests/VelEffController"); + EXPECT_EQ(state2.claimed_resources.size(), 2); + EXPECT_EQ(state2.claimed_resources[0].hardware_interface, "hardware_interface::VelocityJointInterface"); + ASSERT_EQ(state2.claimed_resources[0].resources.size(), 2); + EXPECT_EQ(state2.claimed_resources[0].resources[0], "test_joint1"); + EXPECT_EQ(state2.claimed_resources[0].resources[1], "test_joint2"); + EXPECT_EQ(state2.claimed_resources[1].hardware_interface, "hardware_interface::EffortJointInterface"); + ASSERT_EQ(state2.claimed_resources[1].resources.size(), 1); + EXPECT_EQ(state2.claimed_resources[1].resources[0], "test_joint4"); + } + + // Stop running controller + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + switch_client.call(srv); + } + + // Unload controllers + { + UnloadController srv; + srv.request.name = "my_controller"; + unload_client.call(srv); + srv.request.name = "vel_eff_controller"; + unload_client.call(srv); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "ControllerManagerTestNode"); + + ros::AsyncSpinner spinner(1); + + // wait for services + ROS_INFO("Waiting for service"); + ros::service::waitForService("/controller_manager/load_controller"); + ROS_INFO("Start tests"); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/combined_robot_hw_tests/test/cm_test.test b/combined_robot_hw_tests/test/cm_test.test new file mode 100644 index 000000000..f36390ae5 --- /dev/null +++ b/combined_robot_hw_tests/test/cm_test.test @@ -0,0 +1,68 @@ + + + robot_hardware: + - my_robot_hw_1 + - my_robot_hw_2 + - my_robot_hw_3 + - my_robot_hw_4 + my_robot_hw_1: + type: combined_robot_hw_tests/MyRobotHW1 + my_robot_hw_2: + type: combined_robot_hw_tests/MyRobotHW2 + joints: + - test_joint4 + - test_joint5 + - hiDOF_joint1 + - hiDOF_joint2 + my_robot_hw_3: + type: combined_robot_hw_tests/MyRobotHW3 + robot_description: robot_description + joint_name_filter: right_arm + my_robot_hw_4: + type: combined_robot_hw_tests/MyRobotHW4 + my_controller: + type: controller_manager_tests/EffortTestController + my_controller2: + type: controller_manager_tests/EffortTestController + non_existent_interface_controller: + type: controller_manager_tests/MyDummyController + vel_eff_controller: + type: controller_manager_tests/VelEffController + velocity_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint4 + non_existent_resource_controller: + type: controller_manager_tests/VelEffController + velocity_joints: + - test_joint1 + - test_joint2 + effort_joints: + - nonexistent_resource + self_conflict_controller: + type: controller_manager_tests/VelEffController + velocity_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint1 + optional_interfaces_controller: + type: controller_manager_tests/PosEffOptController + position_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint1 + non_existing_multi_interface_controller: + type: controller_manager_tests/PosEffController + position_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint1 + + + + + diff --git a/combined_robot_hw_tests/test/combined_robot_hw_test.cpp b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp new file mode 100644 index 000000000..e16c654a1 --- /dev/null +++ b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp @@ -0,0 +1,206 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 +#include + +#include +#include +#include +#include + +using combined_robot_hardware::CombinedRobotHW; + +TEST(CombinedRobotHWTests, combinationOk) +{ + ros::NodeHandle nh; + + CombinedRobotHW robot_hw; + bool init_success = robot_hw.init(nh, nh); + ASSERT_TRUE(init_success); + + hardware_interface::JointStateInterface* js_interface = robot_hw.get(); + hardware_interface::EffortJointInterface* ej_interface = robot_hw.get(); + hardware_interface::VelocityJointInterface* vj_interface = robot_hw.get(); + hardware_interface::ForceTorqueSensorInterface* ft_interface = robot_hw.get(); + hardware_interface::HardwareInterface* plain_hw_interface = robot_hw.get(); + hardware_interface::PositionJointInterface* pj_interface = robot_hw.get(); + + ASSERT_TRUE(js_interface != NULL); + ASSERT_TRUE(ej_interface != NULL); + ASSERT_TRUE(vj_interface != NULL); + ASSERT_TRUE(ft_interface != NULL); + ASSERT_TRUE(plain_hw_interface != NULL); + + // Test that no PositionJointInterface was found + ASSERT_EQ(NULL, pj_interface); + + // Test some handles from my_robot_hw_1 + hardware_interface::JointStateHandle js_handle = js_interface->getHandle("test_joint1"); + hardware_interface::JointHandle ej_handle = ej_interface->getHandle("test_joint1"); + hardware_interface::JointHandle vj_handle = vj_interface->getHandle("test_joint1"); + ASSERT_FLOAT_EQ(1.0, js_handle.getPosition()); + ASSERT_FLOAT_EQ(0.0, ej_handle.getVelocity()); + ASSERT_FLOAT_EQ(3.0, ej_handle.getCommand()); + + // Test some handles from my_robot_hw_3 + js_handle = js_interface->getHandle("right_arm_joint_1"); + ej_handle = ej_interface->getHandle("right_arm_joint_1"); + vj_handle = vj_interface->getHandle("right_arm_joint_1"); + ASSERT_FLOAT_EQ(1.0, js_handle.getPosition()); + ASSERT_FLOAT_EQ(0.0, ej_handle.getVelocity()); + ASSERT_FLOAT_EQ(1.5, ej_handle.getCommand()); + + // Test some handles from my_robot_hw_4 + hardware_interface::ForceTorqueSensorHandle ft_handle = ft_interface->getHandle("ft_sensor_1"); + ASSERT_FLOAT_EQ(0.2, ft_handle.getForce()[2]); + + // Test non-existent handle throws exception + ASSERT_ANY_THROW(ej_interface->getHandle("non_existent_joint")); + + // Test read and write functions + robot_hw.read(); + js_handle = js_interface->getHandle("test_joint1"); + ASSERT_FLOAT_EQ(2.7, js_handle.getPosition()); + ASSERT_FLOAT_EQ(1.2, ft_handle.getForce()[2]); + + ej_handle = ej_interface->getHandle("test_joint1"); + ej_handle.setCommand(3.5); + robot_hw.write(); + ej_handle = ej_interface->getHandle("test_joint2"); + ASSERT_FLOAT_EQ(3.5, ej_handle.getCommand()); +} + +TEST(CombinedRobotHWTests, switchOk) +{ + ros::NodeHandle nh; + + CombinedRobotHW robot_hw; + bool init_success = robot_hw.init(nh, nh); + ASSERT_TRUE(init_success); + + // Test empty list (it is expected to work) + { + std::list start_list; + std::list stop_list; + ASSERT_TRUE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_NO_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + + // Test failure + { + std::list start_list; + std::list stop_list; + hardware_interface::ControllerInfo controller_1; + controller_1.name = "ctrl_1"; + controller_1.type = "some_type"; + hardware_interface::InterfaceResources iface_res_1; + iface_res_1.hardware_interface = "hardware_interface::ForceTorqueSensorInterface"; + iface_res_1.resources.insert("ft_sensor_1"); + controller_1.claimed_resources.push_back(iface_res_1); + start_list.push_back(controller_1); + ASSERT_FALSE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_ANY_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + + // Test existing interfaces and resources + { + std::list start_list; + std::list stop_list; + hardware_interface::ControllerInfo controller_1; + controller_1.name = "ctrl_1"; + controller_1.type = "some_type"; + hardware_interface::InterfaceResources iface_res_1; + iface_res_1.hardware_interface = "hardware_interface::EffortJointInterface"; + iface_res_1.resources.insert("test_joint1"); + iface_res_1.resources.insert("test_joint2"); + iface_res_1.resources.insert("test_joint3"); + iface_res_1.resources.insert("test_joint4"); + controller_1.claimed_resources.push_back(iface_res_1); + hardware_interface::InterfaceResources iface_res_2; + iface_res_2.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_2.resources.insert("test_joint1"); + iface_res_2.resources.insert("test_joint4"); + controller_1.claimed_resources.push_back(iface_res_1); + start_list.push_back(controller_1); + + hardware_interface::ControllerInfo controller_2; + hardware_interface::InterfaceResources iface_res_3; + iface_res_3.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_3.resources.insert("test_joint3"); + iface_res_3.resources.insert("test_joint5"); + controller_2.claimed_resources.push_back(iface_res_3); + start_list.push_back(controller_2); + ASSERT_TRUE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_NO_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + + // Test non-registered interfaces and resources + // (this should also work, as CombinedRobotHW will filter out the non-registered ones) + { + std::list start_list; + std::list stop_list; + hardware_interface::ControllerInfo controller_1; + controller_1.name = "ctrl_1"; + controller_1.type = "some_type"; + hardware_interface::InterfaceResources iface_res_1; + iface_res_1.hardware_interface = "hardware_interface::NonRegisteredInterface"; + iface_res_1.resources.insert("test_joint1"); + iface_res_1.resources.insert("test_joint2"); + iface_res_1.resources.insert("test_joint3"); + iface_res_1.resources.insert("test_joint4"); + controller_1.claimed_resources.push_back(iface_res_1); + hardware_interface::InterfaceResources iface_res_2; + iface_res_2.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_2.resources.insert("test_joint1"); + iface_res_2.resources.insert("non_registered_joint1"); + controller_1.claimed_resources.push_back(iface_res_1); + start_list.push_back(controller_1); + + hardware_interface::ControllerInfo controller_2; + hardware_interface::InterfaceResources iface_res_3; + iface_res_3.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_3.resources.insert("test_joint3"); + iface_res_3.resources.insert("non_registered_joint2"); + controller_2.claimed_resources.push_back(iface_res_3); + start_list.push_back(controller_2); + ASSERT_TRUE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_NO_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "CombinedRobotHWTestNode"); + + int ret = RUN_ALL_TESTS(); + + ros::shutdown(); + return ret; +} diff --git a/combined_robot_hw_tests/test/combined_robot_hw_test.test b/combined_robot_hw_tests/test/combined_robot_hw_test.test new file mode 100644 index 000000000..b963f2548 --- /dev/null +++ b/combined_robot_hw_tests/test/combined_robot_hw_test.test @@ -0,0 +1,25 @@ + + + robot_hardware: + - my_robot_hw_1 + - my_robot_hw_2 + - my_robot_hw_3 + - my_robot_hw_4 + my_robot_hw_1: + type: combined_robot_hw_tests/MyRobotHW1 + my_robot_hw_2: + type: combined_robot_hw_tests/MyRobotHW2 + joints: + - test_joint4 + - test_joint5 + my_robot_hw_3: + type: combined_robot_hw_tests/MyRobotHW3 + robot_description: robot_description + joint_name_filter: right_arm + my_robot_hw_4: + type: combined_robot_hw_tests/MyRobotHW4 + + + + + \ No newline at end of file diff --git a/combined_robot_hw_tests/test_robot_hw_plugin.xml b/combined_robot_hw_tests/test_robot_hw_plugin.xml new file mode 100644 index 000000000..43384c87b --- /dev/null +++ b/combined_robot_hw_tests/test_robot_hw_plugin.xml @@ -0,0 +1,25 @@ + + + + A type of RobotHW + + + + + + A type of RobotHW + + + + + + A type of RobotHW + + + + + + A type of RobotHW + + + diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 1c55c3452..392fb58c0 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -26,7 +26,7 @@ // POSSIBILITY OF SUCH DAMAGE. ////////////////////////////////////////////////////////////////////////////// -/// \author Wim Meussen, Adolfo Rodriguez Tsouroukdissian, Kelsey P. Hawkins +/// \author Wim Meussen, Adolfo Rodriguez Tsouroukdissian, Kelsey P. Hawkins, Toni Oliver #ifndef HARDWARE_INTERFACE_INTERFACE_MANAGER_H #define HARDWARE_INTERFACE_INTERFACE_MANAGER_H @@ -79,6 +79,22 @@ struct CheckIsResourceManager { // calls ResourceManager::concatManagers if C is a ResourceManager static const void callConcatManagers(typename std::vector& managers, T* result) { callCM(managers, result, 0); } + + + // method called if C is a ResourceManager + template + static std::vector callGR(C* iface, typename C::resource_manager_type*) + { + return iface->getNames(); + } + + // method called if C is not a ResourceManager + template + static std::vector callGR(T* iface, ...) {} + + // calls ResourceManager::concatManagers if C is a ResourceManager + static std::vector callGetResources(T* iface) + { return callGR(iface, 0); } }; class InterfaceManager @@ -101,7 +117,15 @@ class InterfaceManager { ROS_WARN_STREAM("Replacing previously registered interface '" << iface_name << "'."); } - interfaces_[internal::demangledTypeName()] = iface; + interfaces_[iface_name] = iface; + + std::vector resources; + if(CheckIsResourceManager::value) + { + // it is a ResourceManager. Get the names of the resources + resources = CheckIsResourceManager::callGetResources(iface); + } + resources_[iface_name] = resources; } void registerInterfaceManager(InterfaceManager* iface_man) @@ -198,16 +222,37 @@ class InterfaceManager return out; } + /** + * \brief Get the resource names registered to an interface, specified by type + * (as this class only stores one interface per type) + * + * \param iface_type A string with the demangled type name of the interface + * \return A vector of resource names registered to this interface + */ + std::vector getInterfaceResources(std::string iface_type) const + { + std::vector out; + ResourceMap::const_iterator it = resources_.find(iface_type); + if(it != resources_.end()) + { + out = it->second; + } + return out; + } + protected: typedef std::map InterfaceMap; typedef std::vector InterfaceManagerVector; typedef std::map SizeMap; + typedef std::map > ResourceMap; InterfaceMap interfaces_; InterfaceMap interfaces_combo_; InterfaceManagerVector interface_managers_; SizeMap num_ifaces_registered_; boost::ptr_vector interface_destruction_list_; + /// This will allow us to check the resources based on the demangled type name of the interface + ResourceMap resources_; }; } // namespace diff --git a/hardware_interface/include/hardware_interface/robot_hw.h b/hardware_interface/include/hardware_interface/robot_hw.h index da6e30b01..3657a8ed6 100644 --- a/hardware_interface/include/hardware_interface/robot_hw.h +++ b/hardware_interface/include/hardware_interface/robot_hw.h @@ -36,6 +36,7 @@ #include #include #include +#include namespace hardware_interface { @@ -61,6 +62,23 @@ class RobotHW : public InterfaceManager } + virtual ~RobotHW() + { + + } + + /** \brief The init function is called to initialize the RobotHW from a + * non-realtime thread. + * + * \param root_nh A NodeHandle in the root of the caller namespace. + * + * \param robot_hw_nh A NodeHandle in the namespace from which the RobotHW + * should read its configuration. + * + * \returns True if initialization was successful + */ + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) {return true;} + /** \name Resource Management *\{*/ @@ -128,6 +146,16 @@ class RobotHW : public InterfaceManager */ virtual void doSwitch(const std::list& /*start_list*/, const std::list& /*stop_list*/) {} + + /** + * Reads data from the robot HW + */ + virtual void read() {} + + /** + * Writes data to the robot HW + */ + virtual void write() {} }; }