diff --git a/README.md b/README.md index bd974adb1..0412b098b 100644 --- a/README.md +++ b/README.md @@ -284,7 +284,6 @@ The docker-compose setup is prepared for usage of driver with the official UR si ## Expected Changes in the Near Future -- Using upstream `force_torque_sensor_broadcaster` (ros-controls/ros2_controllers#152) - Trajectory control currently only supports position commands. In the future, velocity control will be added. diff --git a/ur_bringup/config/ur_controllers.yaml b/ur_bringup/config/ur_controllers.yaml index 843eb82e8..67f507c9f 100644 --- a/ur_bringup/config/ur_controllers.yaml +++ b/ur_bringup/config/ur_controllers.yaml @@ -12,7 +12,7 @@ controller_manager: type: ur_controllers/SpeedScalingStateBroadcaster force_torque_sensor_broadcaster: - type: ur_controllers/ForceTorqueStateBroadcaster + type: force_torque_sensor_broadcaster/ForceTorqueStateBroadcaster joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index f39046274..937d7efac 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -24,6 +24,7 @@ rclpy controller_manager + force_torque_sensor_broadcaster joint_state_publisher launch launch_ros diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index f5ff0cd8c..ac564d259 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -50,7 +50,6 @@ include_directories(include) add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp - src/force_torque_sensor_broadcaster.cpp src/gpio_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index 3cfbe2a9f..f0058ab55 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -1,9 +1,4 @@ - - - The force torque state controller publishes the current force and torques - - This controller publishes the readings of all available speed scaling factors. diff --git a/ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp b/ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp deleted file mode 100644 index 680d1e01d..000000000 --- a/ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright (c) 2021, PickNik, 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 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. - -#ifndef UR_CONTROLLERS__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ -#define UR_CONTROLLERS__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ - -#include -#include -#include -#include - -#include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/duration.hpp" - -namespace ur_controllers -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -using FbkType = geometry_msgs::msg::WrenchStamped; - -struct FTStateControllerParams -{ - FTStateControllerParams() - { - sensor_name_ = ""; - state_interfaces_names_ = {}; - frame_id = ""; - topic_name = ""; - } - - std::string sensor_name_; - std::vector state_interfaces_names_; - std::string frame_id; - std::string topic_name; -}; - -/** - * \brief Force torque state controller. - * - * This class forwards the command signal down to a set of joints - * on the specified interface. - * - * \param topic_name Name of the topic to publish WrenchStamped msg - * \param frame_id Frame of the ft sensor - * \param sensor_name Names of the sensor which data should be published . - * \param state_interface_names Names of the state interfaces in the sensor. - * - * Publishes to: - * - \b topic_name (geometry_msg::msg::WrenchStamped) : Sensor data. - */ -class ForceTorqueStateBroadcaster : public controller_interface::ControllerInterface -{ -public: - ForceTorqueStateBroadcaster(); - - 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; - -protected: - // param storage - FTStateControllerParams fts_params_; - std::shared_ptr> wrench_state_publisher_; - geometry_msgs::msg::WrenchStamped wrench_state_msg_; -}; -} // namespace ur_controllers - -#endif // UR_CONTROLLERS__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ diff --git a/ur_controllers/src/force_torque_sensor_broadcaster.cpp b/ur_controllers/src/force_torque_sensor_broadcaster.cpp deleted file mode 100644 index 9820eb86a..000000000 --- a/ur_controllers/src/force_torque_sensor_broadcaster.cpp +++ /dev/null @@ -1,173 +0,0 @@ -// Copyright (c) 2021, PickNik, 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 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. - -#include -#include - -#include "ur_controllers/force_torque_sensor_broadcaster.hpp" - -namespace ur_controllers -{ -ForceTorqueStateBroadcaster::ForceTorqueStateBroadcaster() - : controller_interface::ControllerInterface(), wrench_state_publisher_(nullptr) -{ -} - -CallbackReturn ForceTorqueStateBroadcaster::on_init() -{ - try { - auto_declare>("state_interface_names", {}); - auto_declare("sensor_name", ""); - auto_declare("topic_name", ""); - auto_declare("frame_id", ""); - } catch (const std::exception& e) { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration ForceTorqueStateBroadcaster::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::NONE; - return config; -} - -controller_interface::InterfaceConfiguration ForceTorqueStateBroadcaster::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto& si : fts_params_.state_interfaces_names_) { - config.names.push_back(fts_params_.sensor_name_ + "/" + si); - } - - return config; -} - -controller_interface::return_type -ur_controllers::ForceTorqueStateBroadcaster::update(const rclcpp::Time& time, const rclcpp::Duration& /*period*/) -{ - geometry_msgs::msg::Vector3 f_vec; - geometry_msgs::msg::Vector3 t_vec; - - if (state_interfaces_.size() != fts_params_.state_interfaces_names_.size()) - return controller_interface::return_type::ERROR; - - for (auto index = 0ul; index < state_interfaces_.size(); ++index) { - switch (index) { - case 0: - f_vec.set__x(state_interfaces_[index].get_value()); - break; - case 1: - f_vec.set__y(state_interfaces_[index].get_value()); - break; - case 2: - f_vec.set__z(state_interfaces_[index].get_value()); - break; - case 3: - t_vec.set__x(state_interfaces_[index].get_value()); - break; - case 4: - t_vec.set__y(state_interfaces_[index].get_value()); - break; - case 5: - t_vec.set__z(state_interfaces_[index].get_value()); - break; - default: - break; - } - } - - wrench_state_msg_.header.stamp = time; - wrench_state_msg_.header.frame_id = fts_params_.frame_id; - - // update wrench state message - wrench_state_msg_.wrench.set__force(f_vec); - wrench_state_msg_.wrench.set__torque(t_vec); - - // publish - wrench_state_publisher_->publish(wrench_state_msg_); - - return controller_interface::return_type::OK; -} - -CallbackReturn ForceTorqueStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) -{ - fts_params_.state_interfaces_names_ = node_->get_parameter("state_interface_names").as_string_array(); - - if (fts_params_.state_interfaces_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'state_interface_names' parameter was empty"); - return CallbackReturn::ERROR; - } - - fts_params_.sensor_name_ = node_->get_parameter("sensor_name").as_string(); - if (fts_params_.sensor_name_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter was empty"); - return CallbackReturn::ERROR; - } - - fts_params_.topic_name = node_->get_parameter("topic_name").as_string(); - if (fts_params_.topic_name.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'topic_name' parameter was empty"); - return CallbackReturn::ERROR; - } - - fts_params_.frame_id = node_->get_parameter("frame_id").as_string(); - if (fts_params_.frame_id.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter was empty"); - return CallbackReturn::ERROR; - } - - try { - // register ft sensor data publisher - wrench_state_publisher_ = get_node()->create_publisher( - fts_params_.topic_name, rclcpp::SystemDefaultsQoS()); - } catch (...) { - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -CallbackReturn ForceTorqueStateBroadcaster::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - return CallbackReturn::SUCCESS; -} - -CallbackReturn ForceTorqueStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - return CallbackReturn::SUCCESS; -} - -} // namespace ur_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS(ur_controllers::ForceTorqueStateBroadcaster, controller_interface::ControllerInterface)