diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 04763b5e96..0eb6800878 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -12,8 +12,11 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) +find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) add_library( hardware_interface @@ -26,26 +29,41 @@ add_library( target_include_directories( hardware_interface PUBLIC - include) + include +) ament_target_dependencies( hardware_interface control_msgs rclcpp rcpputils ) - # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +add_library( + component_parser + src/utils/component_parser.cpp +) +target_include_directories( + component_parser + PUBLIC + include +) +ament_target_dependencies( + component_parser + TinyXML2 +) + +target_compile_definitions(component_parser PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") + install( DIRECTORY include/ DESTINATION include ) install( - TARGETS - hardware_interface + TARGETS hardware_interface component_parser RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib) @@ -64,6 +82,11 @@ if(BUILD_TESTING) target_include_directories(test_robot_hardware_interfaces PRIVATE include) target_link_libraries(test_robot_hardware_interfaces hardware_interface) + + ament_add_gmock(test_component_parser test/test_component_parser.cpp) + target_link_libraries(test_component_parser component_parser) + ament_target_dependencies(test_component_parser TinyXML2) + ament_add_gmock(test_register_actuators test/test_register_actuators.cpp) target_include_directories(test_register_actuators PRIVATE include) target_link_libraries(test_register_actuators hardware_interface) @@ -73,14 +96,19 @@ if(BUILD_TESTING) target_include_directories(test_actuator_handle PRIVATE include) target_link_libraries(test_actuator_handle hardware_interface) ament_target_dependencies(test_actuator_handle rcpputils) - endif() +ament_export_dependencies( + rclcpp + rcpputils +) ament_export_include_directories( include ) ament_export_libraries( - hardware_interface) + hardware_interface + component_parser +) ament_export_dependencies( control_msgs) ament_package() diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp new file mode 100644 index 0000000000..a1d82170be --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_info.hpp @@ -0,0 +1,103 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ + +#include +#include +#include + +namespace hardware_interface +{ + +/** + * \brief constants for types of components. + */ +constexpr const auto robotType = "robot"; +constexpr const auto actuatorType = "actuator"; +constexpr const auto sensorType = "sensor"; + +/** + * \brief This structure stores information about components defined in a robot's URDF. + */ +struct ComponentInfo +{ + /** + * \brief name of the component. + */ + std::string name; + /** + * \brief type of the component: sensor or actuator. + */ + std::string type; + /** + * \brief component's class, which will be dynamically loaded. + */ + std::string class_type; + /** + * \brief joint where component is placed. + */ + std::string joint; + /** + * \brief name of the interface, e.g. "position", "velocity", etc. for meaning of data this component holds. + */ + std::vector interface_names; + /** + * \brief (optional) key-value pairs of components parameters. + */ + std::unordered_map parameters; + + /** + * \brief (optional) hardware class of the component that will be dynamically loaded. If not defined, the system's hardware class has to be defined. + */ + std::string hardware_class_type; + /** + * \brief (optional) key-value pairs for components hardware. + */ + std::unordered_map hardware_parameters; +}; + +/** + * \brief This structure stores informations about system defined in robot's URDF, i.e. "ros2_control"-tag. + */ +struct SystemInfo +{ + /** + * \brief name of the system. + */ + std::string name; + /** + * \brief type of the system: robot, actuator or sensor. Note: URDF always needs a "robot" tag, nevertheless in terms of ros2_control, it can hold a definition for an actuator or sensor. + */ + std::string type; + /** + * \brief (optional) hardware class of the system, which will be dynamically loaded. If not defined, a hardware class for each subcomponent has to be defined. + */ + std::string hardware_class_type; + /** + * \brief (optional) key-value pairs for systems hardware. + */ + std::unordered_map hardware_parameters; + + /** + * \brief list of subcomponents in the system, i.e., list of sensors and actuators. + */ + std::vector subcomponents; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp new file mode 100644 index 0000000000..e40f97c797 --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp @@ -0,0 +1,70 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class JointInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + JointInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~JointInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const ComponentInfo & actuator_info) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::string get_interface_name() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + bool is_started() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read(double & data) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type write(const double & data) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp new file mode 100644 index 0000000000..22d195acf2 --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp @@ -0,0 +1,63 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ + +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class SensorInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + SensorInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~SensorInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const ComponentInfo & sensor_info) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::string get_interface_name() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + bool is_started() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read(double & data) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp new file mode 100644 index 0000000000..6b460357be --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp @@ -0,0 +1,70 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class SystemInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + SystemInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~SystemInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const SystemInfo & system_info) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::vector get_interface_names() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + bool is_started() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read(std::vector & data) = 0; + + return_type + virtual + write(const std::vector & data) = 0; +}; + +typedef SystemInterface RobotInterface; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp new file mode 100644 index 0000000000..068cd55df3 --- /dev/null +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -0,0 +1,89 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__JOINT_HPP_ +#define HARDWARE_INTERFACE__JOINT_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/component_interfaces/joint_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class Joint final +{ +public: + Joint() = default; + + explicit Joint(std::unique_ptr & impl) + : impl_(std::move(impl)) + {} + + ~Joint() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & actuator_info) + { + return impl_->configure(actuator_info); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + bool is_started() const + { + return impl_->is_started(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read(double & data) + { + return impl_->read(data); + } + + HARDWARE_INTERFACE_PUBLIC + return_type write(const double & data) + { + return impl_->write(data); + } + + HARDWARE_INTERFACE_PUBLIC + std::string get_interface_name() const + { + return impl_->get_interface_name(); + } + +private: + std::unique_ptr impl_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__JOINT_HPP_ diff --git a/hardware_interface/include/hardware_interface/robot_hardware.hpp b/hardware_interface/include/hardware_interface/robot_hardware.hpp index be997ae1e7..77622d1135 100644 --- a/hardware_interface/include/hardware_interface/robot_hardware.hpp +++ b/hardware_interface/include/hardware_interface/robot_hardware.hpp @@ -19,12 +19,15 @@ #include #include +#include "hardware_interface/joint.hpp" #include "control_msgs/msg/dynamic_joint_state.hpp" #include "hardware_interface/actuator_handle.hpp" #include "hardware_interface/joint_command_handle.hpp" #include "hardware_interface/joint_state_handle.hpp" #include "hardware_interface/operation_mode_handle.hpp" #include "hardware_interface/robot_hardware_interface.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -85,11 +88,15 @@ class RobotHardware : public RobotHardwareInterface get_registered_operation_mode_handles(); HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t register_actuator( + return_type + configure(const std::string & urdf_string); + + HARDWARE_INTERFACE_PUBLIC + return_type register_actuator( const std::string & name, const std::string & interface_name, double default_value = 0.0); HARDWARE_INTERFACE_PUBLIC - hardware_interface_ret_t get_actuator_handle(ActuatorHandle & actuator_handle); + return_type get_actuator_handle(ActuatorHandle & actuator_handle); HARDWARE_INTERFACE_PUBLIC const std::vector & get_registered_actuator_names(); diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp new file mode 100644 index 0000000000..6578f5d422 --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -0,0 +1,77 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef HARDWARE_INTERFACE__SENSOR_HPP_ +#define HARDWARE_INTERFACE__SENSOR_HPP_ + +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/component_interfaces/sensor_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class Sensor final +{ +public: + Sensor() = default; + + explicit Sensor(std::unique_ptr impl) + : impl_(std::move(impl)) + {} + + virtual ~Sensor() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & sensor_info) + { + return impl_->configure(sensor_info); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read(double & data) + { + return impl_->read(data); + } + + HARDWARE_INTERFACE_PUBLIC + std::string get_interface_name() + { + return impl_->get_interface_name(); + } + +private: + std::unique_ptr impl_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SENSOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp new file mode 100644 index 0000000000..eecf5b68b5 --- /dev/null +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -0,0 +1,88 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__SYSTEM_HPP_ +#define HARDWARE_INTERFACE__SYSTEM_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/component_interfaces/system_interface.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class System final +{ +public: + explicit System(std::unique_ptr impl) + : impl_(std::move(impl)) + {} + + virtual ~System() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const SystemInfo & system_info) + { + return impl_->configure(system_info); + } + + HARDWARE_INTERFACE_PUBLIC + std::vector get_interface_names() + { + return impl_->get_interface_names(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + bool is_started() + { + return impl_->is_started(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read(std::vector & data) + { + return impl_->read(data); + } + + HARDWARE_INTERFACE_PUBLIC + return_type write(const std::vector & data) + { + return impl_->write(data); + } + +private: + std::unique_ptr impl_; +}; + +typedef System Robot; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SYSTEM_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp new file mode 100644 index 0000000000..7292f28229 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -0,0 +1,25 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ + +namespace hardware_interface +{ +constexpr const auto HW_IF_POSITION = "position"; +constexpr const auto HW_IF_VELOCITY = "velocity"; +constexpr const auto HW_IF_EFFORT = "effort"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ diff --git a/hardware_interface/include/hardware_interface/utils/component_parser.hpp b/hardware_interface/include/hardware_interface/utils/component_parser.hpp new file mode 100644 index 0000000000..eb04709222 --- /dev/null +++ b/hardware_interface/include/hardware_interface/utils/component_parser.hpp @@ -0,0 +1,63 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__UTILS__COMPONENT_PARSER_HPP_ +#define HARDWARE_INTERFACE__UTILS__COMPONENT_PARSER_HPP_ + +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace utils +{ + +/** + * \brief Search XML snippet from URDF for informations about a control component. + * + * \param urdf string with robot's URDF + * \return robot_control_components::ComponentInfo filled with informations about the robot + * \throws std::runtime_error if a robot attribute or tag is not found + */ +HARDWARE_INTERFACE_PUBLIC +SystemInfo parse_system_from_urdf(const std::string & urdf); + +/** + * \brief Search XML snippet from URDF for informations about a control component. + * + * \param component_it pointer to the iterator where component info should be found + * \return robot_control_components::ComponentInfo filled with informations about component + * \throws std::runtime_error if a component attribute or tag is not found + */ +HARDWARE_INTERFACE_PUBLIC +ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it); + +/** + * \brief Search XML snippet from URDF for parameters. + * + * \param params_it pointer to the iterator where parameters info should be found + * \return std::map< std::__cxx11::string, std::__cxx11::string > key-value map with parameters + * \throws std::runtime_error if a component attribute or tag is not found + */ +HARDWARE_INTERFACE_PUBLIC +std::unordered_map parse_parameters_from_xml( + const tinyxml2::XMLElement * params_it); + +} // namespace utils +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__UTILS__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 74b50f4098..742064709c 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -9,9 +9,11 @@ ament_cmake + control_msgs rclcpp rcpputils - control_msgs + tinyxml2_vendor + TinyXML2 ament_cmake_gmock ament_lint_auto diff --git a/hardware_interface/src/robot_hardware.cpp b/hardware_interface/src/robot_hardware.cpp index 8e99c34bba..99490c879d 100644 --- a/hardware_interface/src/robot_hardware.cpp +++ b/hardware_interface/src/robot_hardware.cpp @@ -21,6 +21,7 @@ #include "hardware_interface/macros.hpp" #include "hardware_interface/operation_mode_handle.hpp" #include "rclcpp/rclcpp.hpp" +#include "hardware_interface/utils/component_parser.hpp" namespace { diff --git a/hardware_interface/src/utils/component_parser.cpp b/hardware_interface/src/utils/component_parser.cpp new file mode 100644 index 0000000000..95dd397d43 --- /dev/null +++ b/hardware_interface/src/utils/component_parser.cpp @@ -0,0 +1,200 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/utils/component_parser.hpp" + +namespace +{ +constexpr const auto kRobotTag = "robot"; +constexpr const auto kROS2ControlTag = "ros2_control"; +constexpr const auto kHardwareTag = "hardware"; +constexpr const auto kClassTypeTag = "classType"; +constexpr const auto kParamTag = "param"; +constexpr const auto kJointTag = "joint"; +constexpr const auto kInterfaceNameTag = "interfaceName"; + +// For complete reference of syntax - not used in parser +// constexpr const auto kJointTag = "joint"; +// constexpr const auto kSensorTag = "sensor"; +} // namespace + +namespace hardware_interface +{ +namespace utils +{ + +SystemInfo parse_system_from_urdf(const std::string & urdf) +{ + // Check if everything OK with URDF string + if (urdf.empty()) { + throw std::runtime_error("empty URDF passed to robot"); + } + tinyxml2::XMLDocument doc; + if (!doc.Parse(urdf.c_str()) && doc.Error()) { + throw std::runtime_error("invalid URDF passed in to robot parser"); + } + + // Find robot tag + const tinyxml2::XMLElement * robot_it = doc.RootElement(); + + if (std::string(kRobotTag).compare(robot_it->Name())) { + throw std::runtime_error("the robot tag is not root element in URDF"); + } + + SystemInfo system; + const tinyxml2::XMLAttribute * attr; + + attr = robot_it->FindAttribute("name"); + if (!attr) { + throw std::runtime_error("no robot name attribute set"); + } + system.name = robot_it->Attribute("name"); + + const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); + if (!ros2_control_it) { + throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); + } + attr = ros2_control_it->FindAttribute("name"); + if (!attr) { + throw std::runtime_error("no attribute name in " + std::string(kROS2ControlTag) + " tag"); + } + attr = ros2_control_it->FindAttribute("type"); + if (!attr) { + throw std::runtime_error("no attribute type in " + std::string(kROS2ControlTag) + " tag"); + } + system.type = ros2_control_it->Attribute("type"); + + // Parse everything under ros2_control tag + system.hardware_class_type = ""; + const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(); + while (ros2_control_child_it) { + if (!std::string(kHardwareTag).compare(ros2_control_child_it->Name())) { + const auto * type_it = ros2_control_child_it->FirstChildElement(kClassTypeTag); + system.hardware_class_type = type_it->GetText(); + const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag); + if (params_it) { + system.hardware_parameters = parse_parameters_from_xml(params_it); + } + } else { + system.subcomponents.push_back(parse_component_from_xml(ros2_control_child_it) ); + } + + ros2_control_child_it = ros2_control_child_it->NextSiblingElement(); + } + + return system; +} + +ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) +{ + ComponentInfo component; + const tinyxml2::XMLAttribute * attr; + + // Find name, type and class for component + component.type = component_it->Name(); + attr = component_it->FindAttribute("name"); + if (!attr) { + throw std::runtime_error("no name attribute set in " + component.type + " tag"); + } + component.name = component_it->Attribute("name"); + + const auto * classType_it = component_it->FirstChildElement(kClassTypeTag); + if (!classType_it) { + throw std::runtime_error("no class type tag found in " + component.name); + } + component.class_type = classType_it->GetText(); + if (component.class_type.empty()) { + throw std::runtime_error("no class type specified in " + component.name); + } + + // Find joints and itnerface names in control component + const auto * joint_it = component_it->FirstChildElement(kJointTag); + if (!joint_it) { + throw std::runtime_error("no joint element found in " + component.name); + } + attr = joint_it->FindAttribute("name"); + if (!attr) { + throw std::runtime_error("no joint attribute name found in " + component.name); + } + component.joint = joint_it->Attribute("name"); + const auto * interface_name_it = joint_it->FirstChildElement(kInterfaceNameTag); + if (!interface_name_it) { + throw std::runtime_error( + "no interface names found for " + component.joint + " in " + component.name); + } + while (interface_name_it) { + const std::string interface_name = interface_name_it->GetText(); + if (interface_name.empty()) { + throw std::runtime_error( + "no interface name value in " + component.joint + " of " + component.name); + } + component.interface_names.push_back(interface_name); + + interface_name_it = interface_name_it->NextSiblingElement(kInterfaceNameTag); + } + + // Parse paramter tags + const auto * params_it = component_it->FirstChildElement(kParamTag); + if (params_it) { + component.parameters = parse_parameters_from_xml(params_it); + } + + // Parse hardware tag + const auto * hardware_it = component_it->FirstChildElement(kHardwareTag); + if (hardware_it) { + const auto * type_it = hardware_it->FirstChildElement(kClassTypeTag); + component.hardware_class_type = type_it->GetText(); + const auto * params_it = hardware_it->FirstChildElement(kParamTag); + if (params_it) { + component.hardware_parameters = parse_parameters_from_xml(params_it); + } + } else { + component.hardware_class_type = ""; + } + + return component; +} + +std::unordered_map parse_parameters_from_xml( + const tinyxml2::XMLElement * params_it) +{ + std::unordered_map parameters; + const tinyxml2::XMLAttribute * attr; + + while (params_it) { + // Fill the map with parameters + attr = params_it->FindAttribute("name"); + if (!attr) { + throw std::runtime_error("no parameter name attribute set in param tag"); + } + const std::string parameter_name = params_it->Attribute("name"); + const std::string parameter_value = params_it->GetText(); + if (parameter_value.empty()) { + throw std::runtime_error("no parameter value set for " + parameter_name); + } + parameters[parameter_name] = parameter_value; + + params_it = params_it->NextSiblingElement(kParamTag); + } + return parameters; +} + +} // namespace utils +} // namespace hardware_interface diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp new file mode 100644 index 0000000000..bf10cd3237 --- /dev/null +++ b/hardware_interface/test/test_component_parser.cpp @@ -0,0 +1,359 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/utils/component_parser.hpp" + +using namespace ::testing; // NOLINT + +class TestComponentParser : public Test +{ +protected: + void SetUp() override + { + urdf_xml_head_ = + R"( + + + + + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Gray + + + Gazebo/Gray + + + Gazebo/Gray + + + + + +)"; + + urdf_xml_tail_ = + R"( + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + VelocityJointInterface + + + 1 + + + +)"; + + valid_urdf_ros2_control_joints_only_ = + R"( + + + ros2_control_demo_hardware/DemoRobotHardwareMinimal + 2 + 2 + + + ros2_control_components/PositionActuator + + position + + True + -1 + 1 + + + ros2_control_components/PositionActuator + + position + + True + -1 + 1 + + +)"; + + valid_urdf_ros2_control_joints_sensors_ = + R"( + + + ros2_control_demo_hardware/DemoRobotHardwareMinimal + 2 + 2 + + + ros2_control_components/PositionActuator + + position + + True + -1 + 1 + + + ros2_control_components/PositionSensor + + position + + -5 + 1 + + +)"; + + valid_urdf_ros2_control_joints_sensors_hardware_ = + R"( + + + ros2_control_components/PositionActuator + + position + + True + -1 + 1 + + ros2_control_demo_hardware/DemoActuatorHardware + 1.23 + + + + ros2_control_components/PositionSensor + + position + + -5 + 1 + + ros2_control_demo_hardware/DemoSensorHardware + 3 + + + +)"; + } + + std::string urdf_xml_head_, urdf_xml_tail_; + std::string valid_urdf_ros2_control_joints_only_, valid_urdf_ros2_control_joints_sensors_; + std::string valid_urdf_ros2_control_joints_sensors_hardware_; +}; + +using hardware_interface::utils::parse_system_from_urdf; + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuators) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_joints_only_ + + urdf_xml_tail_; + const auto system_info = parse_system_from_urdf(urdf_to_test); + + EXPECT_EQ(system_info.name, "MinimalRobot"); + EXPECT_EQ(system_info.type, "robot"); + EXPECT_EQ(system_info.hardware_class_type, "ros2_control_demo_hardware/DemoRobotHardwareMinimal"); + ASSERT_THAT(system_info.subcomponents, SizeIs(2)); + ASSERT_THAT(system_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(system_info.hardware_parameters.at("write_for_sec"), "2"); + + EXPECT_EQ(system_info.subcomponents[0].name, "joint1_position_actuator"); + EXPECT_EQ(system_info.subcomponents[0].type, "joint"); + EXPECT_EQ(system_info.subcomponents[0].class_type, "ros2_control_components/PositionActuator"); + EXPECT_EQ(system_info.subcomponents[0].joint, "joint1"); + ASSERT_THAT(system_info.subcomponents[0].parameters, SizeIs(3)); + + EXPECT_EQ(system_info.subcomponents[1].parameters.at("can_read"), "True"); + EXPECT_EQ(system_info.subcomponents[1].hardware_class_type, ""); + ASSERT_THAT(system_info.subcomponents[1].hardware_parameters, SizeIs(0)); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuators_sensors) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_joints_sensors_ + + urdf_xml_tail_; + const auto system_info = parse_system_from_urdf(urdf_to_test); + + EXPECT_EQ(system_info.name, "MinimalRobot"); + EXPECT_EQ(system_info.type, "robot"); + EXPECT_EQ(system_info.hardware_class_type, "ros2_control_demo_hardware/DemoRobotHardwareMinimal"); + ASSERT_THAT(system_info.subcomponents, SizeIs(2)); + ASSERT_THAT(system_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(system_info.hardware_parameters.at("write_for_sec"), "2"); + + EXPECT_EQ(system_info.subcomponents[0].name, "joint1_position_actuator"); + EXPECT_EQ(system_info.subcomponents[0].type, "joint"); + EXPECT_EQ(system_info.subcomponents[0].class_type, "ros2_control_components/PositionActuator"); + EXPECT_EQ(system_info.subcomponents[0].joint, "joint1"); + ASSERT_THAT(system_info.subcomponents[0].parameters, SizeIs(3)); + + EXPECT_EQ(system_info.subcomponents[1].type, "sensor"); + EXPECT_EQ(system_info.subcomponents[1].class_type, "ros2_control_components/PositionSensor"); + EXPECT_EQ(system_info.subcomponents[1].parameters.at("min_values"), "-5"); + EXPECT_EQ(system_info.subcomponents[1].hardware_class_type, ""); + ASSERT_THAT(system_info.subcomponents[1].hardware_parameters, SizeIs(0)); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuators_sensors_hardware) +{ + std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_joints_sensors_hardware_ + + urdf_xml_tail_; + const auto system_info = parse_system_from_urdf(urdf_to_test); + + EXPECT_EQ(system_info.name, "MinimalRobot"); + EXPECT_EQ(system_info.type, "robot"); + EXPECT_EQ(system_info.hardware_class_type, ""); + ASSERT_THAT(system_info.subcomponents, SizeIs(2)); + ASSERT_THAT(system_info.hardware_parameters, SizeIs(0)); + + EXPECT_EQ(system_info.subcomponents[0].name, "joint1_position_actuator"); + EXPECT_EQ(system_info.subcomponents[0].type, "joint"); + EXPECT_EQ(system_info.subcomponents[0].class_type, "ros2_control_components/PositionActuator"); + EXPECT_EQ(system_info.subcomponents[0].joint, "joint1"); + ASSERT_THAT(system_info.subcomponents[0].parameters, SizeIs(3)); + EXPECT_EQ(system_info.subcomponents[0].hardware_class_type, + "ros2_control_demo_hardware/DemoActuatorHardware"); + ASSERT_THAT(system_info.subcomponents[0].hardware_parameters, SizeIs(1)); + EXPECT_EQ(system_info.subcomponents[0].hardware_parameters.at("write_for_sec"), "1.23"); + + EXPECT_EQ(system_info.subcomponents[1].type, "sensor"); + EXPECT_EQ(system_info.subcomponents[1].class_type, "ros2_control_components/PositionSensor"); + EXPECT_EQ(system_info.subcomponents[1].parameters.at("min_values"), "-5"); + EXPECT_EQ(system_info.subcomponents[1].hardware_class_type, + "ros2_control_demo_hardware/DemoSensorHardware"); + ASSERT_THAT(system_info.subcomponents[1].hardware_parameters, SizeIs(1)); + EXPECT_EQ(system_info.subcomponents[1].hardware_parameters.at("read_for_sec"), "2"); +} + +TEST_F(TestComponentParser, empty_string_throws_error) +{ + ASSERT_THROW(parse_system_from_urdf(""), std::runtime_error); +} + +TEST_F(TestComponentParser, empty_urdf_throws_error) +{ + const std::string empty_urdf = + ""; + + ASSERT_THROW(parse_system_from_urdf(empty_urdf), std::runtime_error); +} diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index cb7fc20c4f..e826ae604a 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -193,7 +193,6 @@ TEST_F(TestRobotHardwareInterface, can_get_handles_by_name) EXPECT_EQ( hw::return_type::ERROR, robot_.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); - SetUpNewHandles(); state_handle = nullptr; EXPECT_EQ(hw::return_type::OK, robot_.get_joint_state_handle(NEW_JOINT_NAME, &state_handle)); diff --git a/robot_control_components/CMakeLists.txt b/robot_control_components/CMakeLists.txt new file mode 100644 index 0000000000..57e65bb403 --- /dev/null +++ b/robot_control_components/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.5) +project(robot_control_components) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) + +add_library(${PROJECT_NAME} SHARED + # joints + src/effort_joint.cpp + src/position_joint.cpp + src/velocity_joint.cpp + # sensors + src/position_sensor.cpp + src/velocity_sensor.cpp +) +target_include_directories(${PROJECT_NAME} + PRIVATE include +) +ament_target_dependencies(${PROJECT_NAME} + hardware_interface + pluginlib +) + +pluginlib_export_plugin_description_file( + robot_control_components robot_control_components_plugins.xml) + +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies( + hardware_interface + pluginlib +) +ament_package() diff --git a/robot_control_components/package.xml b/robot_control_components/package.xml new file mode 100644 index 0000000000..1133b99210 --- /dev/null +++ b/robot_control_components/package.xml @@ -0,0 +1,20 @@ + + + + robot_control_components + 0.0.0 + The main package for `ros2_control`-concept testing. The package implements the most important classes and tests them with `demo_robot` to enable functionality and test driven development. + Denis Štogl + Apache License 2.0 + + + hardware_interface + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot_control_components/robot_control_components_plugins.xml b/robot_control_components/robot_control_components_plugins.xml new file mode 100644 index 0000000000..43b0c86ce6 --- /dev/null +++ b/robot_control_components/robot_control_components_plugins.xml @@ -0,0 +1,27 @@ + + + + The effot joint provides virtual representation of effot-only joint. + + + + + The position joint provides virtual representation of position-only joint. + + + + + The velocity joint provides virtual representation of velocity-only joint. + + + + + The position sensor provides virtual representation of position-only sensor. + + + + + The velocity sensor provides virtual representation of velocity-only sensor. + + + diff --git a/robot_control_components/src/effort_joint.cpp b/robot_control_components/src/effort_joint.cpp new file mode 100644 index 0000000000..d41c8dda58 --- /dev/null +++ b/robot_control_components/src/effort_joint.cpp @@ -0,0 +1,67 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "hardware_interface/component_interfaces/joint_interface.hpp" + +namespace robot_control_components +{ + +namespace hw = hardware_interface; + +class EffortJoint : public hardware_interface::JointInterface +{ +public: + hw::return_type configure(const hw::ComponentInfo & /* actuator_info */) + { + return hw::return_type::OK; + } + + std::string get_interface_name() const + { + return ""; + } + + hw::return_type start() + { + return hw::return_type::OK; + } + + hw::return_type stop() + { + return hw::return_type::OK; + } + + bool is_started() const + { + return false; + } + + hw::return_type read(double & /* data */) + { + return hw::return_type::OK; + } + + hw::return_type write(const double & /* data */) + { + return hw::return_type::OK; + } +}; + +} // namespace robot_control_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + robot_control_components::EffortJoint, hardware_interface::JointInterface) diff --git a/robot_control_components/src/position_joint.cpp b/robot_control_components/src/position_joint.cpp new file mode 100644 index 0000000000..e9b59e1e88 --- /dev/null +++ b/robot_control_components/src/position_joint.cpp @@ -0,0 +1,67 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "hardware_interface/component_interfaces/joint_interface.hpp" + +namespace robot_control_components +{ + +namespace hw = hardware_interface; + +class PositionJoint : public hardware_interface::JointInterface +{ +public: + hw::return_type configure(const hw::ComponentInfo & /* actuator_info */) + { + return hw::return_type::OK; + } + + std::string get_interface_name() const + { + return ""; + } + + hw::return_type start() + { + return hw::return_type::OK; + } + + hw::return_type stop() + { + return hw::return_type::OK; + } + + bool is_started() const + { + return false; + } + + hw::return_type read(double & /* data */) + { + return hw::return_type::OK; + } + + hw::return_type write(const double & /* data */) + { + return hw::return_type::OK; + } +}; + +} // namespace robot_control_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + robot_control_components::PositionJoint, hardware_interface::JointInterface) diff --git a/robot_control_components/src/position_sensor.cpp b/robot_control_components/src/position_sensor.cpp new file mode 100644 index 0000000000..a4021dd5df --- /dev/null +++ b/robot_control_components/src/position_sensor.cpp @@ -0,0 +1,62 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "hardware_interface/component_interfaces/sensor_interface.hpp" + +namespace robot_control_components +{ + +namespace hw = hardware_interface; + +class PositionSensor : public hardware_interface::SensorInterface +{ +public: + hw::return_type configure(const hw::ComponentInfo & /* sensor_info */) + { + return hw::return_type::OK; + } + + std::string get_interface_name() const + { + return ""; + } + + hw::return_type start() + { + return hw::return_type::OK; + } + + hw::return_type stop() + { + return hw::return_type::OK; + } + + bool is_started() const + { + return false; + } + + hw::return_type read(double & /* data */) + { + return hw::return_type::OK; + } +}; + +} // namespace robot_control_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + robot_control_components::PositionSensor, hardware_interface::SensorInterface) diff --git a/robot_control_components/src/velocity_joint.cpp b/robot_control_components/src/velocity_joint.cpp new file mode 100644 index 0000000000..2f41b3a96d --- /dev/null +++ b/robot_control_components/src/velocity_joint.cpp @@ -0,0 +1,69 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/component_interfaces/joint_interface.hpp" + +namespace robot_control_components +{ + +namespace hw = hardware_interface; + +class VelocityJoint : public hardware_interface::JointInterface +{ +public: + hw::return_type configure(const hw::ComponentInfo & /* actuator_info */) + { + return hw::return_type::OK; + } + + std::string get_interface_name() const + { + return ""; + } + + hw::return_type start() + { + return hw::return_type::OK; + } + + hw::return_type stop() + { + return hw::return_type::OK; + } + + bool is_started() const + { + return false; + } + + hw::return_type read(double & /* data */) + { + return hw::return_type::OK; + } + + hw::return_type write(const double & /* data */) + { + return hw::return_type::OK; + } +}; + +} // namespace robot_control_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + robot_control_components::VelocityJoint, hardware_interface::JointInterface) diff --git a/robot_control_components/src/velocity_sensor.cpp b/robot_control_components/src/velocity_sensor.cpp new file mode 100644 index 0000000000..2b335f0266 --- /dev/null +++ b/robot_control_components/src/velocity_sensor.cpp @@ -0,0 +1,64 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/component_interfaces/sensor_interface.hpp" + +namespace robot_control_components +{ + +namespace hw = hardware_interface; + +class VelocitySensor : public hardware_interface::SensorInterface +{ +public: + hw::return_type configure(const hw::ComponentInfo & /* sensor_info */) + { + return hw::return_type::OK; + } + + std::string get_interface_name() const + { + return ""; + } + + hw::return_type start() + { + return hw::return_type::OK; + } + + hw::return_type stop() + { + return hw::return_type::OK; + } + + bool is_started() const + { + return false; + } + + hw::return_type read(double & /* data */) + { + return hw::return_type::OK; + } +}; + +} // namespace robot_control_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + robot_control_components::VelocitySensor, hardware_interface::SensorInterface) diff --git a/test_robot_hardware/src/test_robot_hardware.cpp b/test_robot_hardware/src/test_robot_hardware.cpp index 986eeca80d..1dcd182ced 100644 --- a/test_robot_hardware/src/test_robot_hardware.cpp +++ b/test_robot_hardware/src/test_robot_hardware.cpp @@ -17,6 +17,8 @@ #include #include +using hardware_interface::hardware_interface_ret_t; + namespace test_robot_hardware {