-
Notifications
You must be signed in to change notification settings - Fork 250
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Create new URConfigurationController
And moved the get_robot_software_version service in to it.
- Loading branch information
Showing
16 changed files
with
257 additions
and
315 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
89 changes: 89 additions & 0 deletions
89
ur_controllers/include/ur_controllers/ur_configuration_controller.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,89 @@ | ||
| // Copyright 2024, Universal Robots A/S | ||
| // | ||
| // 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 the {copyright_holder} 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 HOLDER 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. | ||
|
|
||
| //---------------------------------------------------------------------- | ||
| /*!\file | ||
| * | ||
| * \author Jacob Larsen jala@universal-robots.com | ||
| * \date 2024-07-11 | ||
| * | ||
| * | ||
| * | ||
| * | ||
| */ | ||
| //---------------------------------------------------------------------- | ||
|
|
||
| #ifndef UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ | ||
| #define UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ | ||
|
|
||
| #include <memory> | ||
|
|
||
| #include <controller_interface/controller_interface.hpp> | ||
|
|
||
| #include "ur_msgs/srv/get_robot_software_version.hpp" | ||
| #include "ur_configuration_controller_parameters.hpp" | ||
|
|
||
| namespace ur_controllers | ||
| { | ||
|
|
||
| enum StateInterfaces | ||
| { | ||
| ROBOT_VERSION_MAJOR, | ||
| ROBOT_VERSION_MINOR, | ||
| ROBOT_VERSION_BUILD, | ||
| ROBOT_VERSION_BUGFIX | ||
| }; | ||
|
|
||
| class URConfigurationController : public controller_interface::ControllerInterface | ||
| { | ||
| public: | ||
| controller_interface::InterfaceConfiguration command_interface_configuration() const override; | ||
|
|
||
| controller_interface::InterfaceConfiguration state_interface_configuration() const override; | ||
|
|
||
| controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; | ||
|
|
||
| CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; | ||
|
|
||
| CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; | ||
|
|
||
| CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; | ||
|
|
||
| CallbackReturn on_init() override; | ||
|
|
||
| private: | ||
| rclcpp::Service<ur_msgs::srv::GetRobotSoftwareVersion>::SharedPtr get_robot_software_version_srv_; | ||
| bool getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr req, | ||
| ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp); | ||
|
|
||
| std::shared_ptr<ur_configuration_controller::ParamListener> param_listener_; | ||
| ur_configuration_controller::Params params_; | ||
| }; | ||
| } // namespace ur_controllers | ||
|
|
||
| #endif // UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,122 @@ | ||
| // Copyright 2024, Universal Robots A/S | ||
| // | ||
| // 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 the {copyright_holder} 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 HOLDER 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. | ||
|
|
||
| //---------------------------------------------------------------------- | ||
| /*!\file | ||
| * | ||
| * \author Jacob Larsen jala@universal-robots.com | ||
| * \date 2024-07-11 | ||
| * | ||
| * | ||
| * | ||
| * | ||
| */ | ||
| //---------------------------------------------------------------------- | ||
|
|
||
| #include <ur_controllers/ur_configuration_controller.hpp> | ||
| namespace ur_controllers | ||
| { | ||
|
|
||
| controller_interface::CallbackReturn URConfigurationController::on_init() | ||
| { | ||
| param_listener_ = std::make_shared<ur_configuration_controller::ParamListener>(get_node()); | ||
| params_ = param_listener_->get_params(); | ||
| return controller_interface::CallbackReturn::SUCCESS; | ||
| } | ||
|
|
||
| controller_interface::CallbackReturn | ||
| URConfigurationController::on_configure(const rclcpp_lifecycle::State& /* previous_state */) | ||
| { | ||
| return controller_interface::CallbackReturn::SUCCESS; | ||
| } | ||
|
|
||
| controller_interface::InterfaceConfiguration URConfigurationController::command_interface_configuration() const | ||
| { | ||
| // No command interfaces currently | ||
| controller_interface::InterfaceConfiguration config; | ||
| config.type = controller_interface::interface_configuration_type::INDIVIDUAL; | ||
|
|
||
| const std::string tf_prefix = params_.tf_prefix; | ||
|
|
||
| return config; | ||
| } | ||
|
|
||
| controller_interface::InterfaceConfiguration URConfigurationController::state_interface_configuration() const | ||
| { | ||
| controller_interface::InterfaceConfiguration config; | ||
| config.type = controller_interface::interface_configuration_type::INDIVIDUAL; | ||
|
|
||
| const std::string tf_prefix = params_.tf_prefix; | ||
|
|
||
| config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major"); | ||
| config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_minor"); | ||
| config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_build"); | ||
| config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_bugfix"); | ||
|
|
||
| return config; | ||
| } | ||
|
|
||
| controller_interface::return_type URConfigurationController::update(const rclcpp::Time& /* time */, | ||
| const rclcpp::Duration& /* period */) | ||
| { | ||
| return controller_interface::return_type::OK; | ||
| } | ||
|
|
||
| controller_interface::CallbackReturn | ||
| URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */) | ||
| { | ||
| get_robot_software_version_srv_ = get_node()->create_service<ur_msgs::srv::GetRobotSoftwareVersion>( | ||
| "~/get_robot_software_version", std::bind(&URConfigurationController::getRobotSoftwareVersion, this, | ||
| std::placeholders::_1, std::placeholders::_2)); | ||
|
|
||
| return controller_interface::CallbackReturn::SUCCESS; | ||
| } | ||
|
|
||
| controller_interface::CallbackReturn | ||
| URConfigurationController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */) | ||
| { | ||
| get_robot_software_version_srv_.reset(); | ||
| return controller_interface::CallbackReturn::SUCCESS; | ||
| } | ||
|
|
||
| bool URConfigurationController::getRobotSoftwareVersion( | ||
| ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/, | ||
| ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp) | ||
| { | ||
| resp->major = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value()); | ||
| resp->minor = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value()); | ||
| resp->bugfix = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value()); | ||
| resp->build = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value()); | ||
|
|
||
| return true; | ||
| } | ||
| } // namespace ur_controllers | ||
|
|
||
| #include "pluginlib/class_list_macros.hpp" | ||
|
|
||
| PLUGINLIB_EXPORT_CLASS(ur_controllers::URConfigurationController, controller_interface::ControllerInterface) |
6 changes: 6 additions & 0 deletions
6
ur_controllers/src/ur_configuration_controller_parameters.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,6 @@ | ||
| ur_configuration_controller: | ||
| tf_prefix: { | ||
| type: string, | ||
| default_value: "", | ||
| description: "Urdf prefix of the corresponding arm" | ||
| } |
Oops, something went wrong.