diff --git a/ur_bringup/config/ur_controllers.yaml b/ur_bringup/config/ur_controllers.yaml index 9fd1b6d2a..fe5daee4b 100644 --- a/ur_bringup/config/ur_controllers.yaml +++ b/ur_bringup/config/ur_controllers.yaml @@ -8,7 +8,7 @@ controller_manager: type: ur_controllers/GPIOController speed_scaling_state_broadcaster: - type: ur_controllers/SpeedScalingStateBroadcaster + type: scaled_controllers/SpeedScalingStateBroadcaster force_torque_sensor_broadcaster: type: ur_controllers/ForceTorqueStateBroadcaster @@ -17,7 +17,7 @@ controller_manager: type: joint_trajectory_controller/JointTrajectoryController scaled_joint_trajectory_controller: - type: ur_controllers/ScaledJointTrajectoryController + type: scaled_controllers/ScaledJointTrajectoryController forward_velocity_controller: type: velocity_controllers/JointGroupVelocityController diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index af7f4eaa7..112851b46 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -25,6 +25,8 @@ controller_manager joint_state_publisher + joint_state_publisher + scaled_controllers launch launch_ros rviz2 diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index f5ff0cd8c..7b0a72fb2 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -48,8 +48,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS 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) diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index 3cfbe2a9f..f1198c77c 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -4,16 +4,6 @@ The force torque state controller publishes the current force and torques - - - This controller publishes the readings of all available speed scaling factors. - - - - - Scaled position-based joint trajectory controller. - - This controller publishes the Tool IO. diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp deleted file mode 100644 index a34bda045..000000000 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2019, FZI Forschungszentrum Informatik -// -// 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. - -//---------------------------------------------------------------------- -/*!\file - * - * \author Marvin Große Besselmann grosse@fzi.de - * \date 2021-02-18 - * - */ -//---------------------------------------------------------------------- -#ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_ -#define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_ - -#include "angles/angles.h" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "joint_trajectory_controller/trajectory.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/duration.hpp" - -namespace ur_controllers -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController -{ -public: - ScaledJointTrajectoryController() = default; - ~ScaledJointTrajectoryController() override = default; - - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; - - controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; - -protected: - struct TimeData - { - TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) - { - } - rclcpp::Time time; - rclcpp::Duration period; - rclcpp::Time uptime; - }; - -private: - double scaling_factor_; - realtime_tools::RealtimeBuffer time_data_; -}; -} // namespace ur_controllers - -#endif // UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp b/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp deleted file mode 100644 index c7c125657..000000000 --- a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2019, FZI Forschungszentrum Informatik -// -// 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. - -//---------------------------------------------------------------------- -/*!\file - * - * \author Marvin Große Besselmann grosse@fzi.de - * \date 2021-02-10 - * - */ -//---------------------------------------------------------------------- - -#ifndef UR_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_ -#define UR_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_ - -#include -#include -#include - -#include "controller_interface/controller_interface.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/duration.hpp" -#include "std_msgs/msg/float64.hpp" - -namespace ur_controllers -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class SpeedScalingStateBroadcaster : public controller_interface::ControllerInterface -{ -public: - SpeedScalingStateBroadcaster(); - - 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: - std::vector sensor_names_; - double publish_rate_; - - std::shared_ptr> speed_scaling_state_publisher_; - std_msgs::msg::Float64 speed_scaling_state_msg_; -}; -} // namespace ur_controllers -#endif // UR_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_ diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp deleted file mode 100644 index 950d52eff..000000000 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ /dev/null @@ -1,249 +0,0 @@ -// Copyright 2019, FZI Forschungszentrum Informatik -// -// 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. - -//---------------------------------------------------------------------- -/*!\file - * - * \author Marvin Große Besselmann grosse@fzi.de - * \date 2021-02-18 - * - */ -//---------------------------------------------------------------------- - -#include -#include - -#include "ur_controllers/scaled_joint_trajectory_controller.hpp" - -namespace ur_controllers -{ -controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf = JointTrajectoryController::state_interface_configuration(); - conf.names.push_back("speed_scaling/speed_scaling_factor"); - return conf; -} - -CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) -{ - TimeData time_data; - time_data.time = node_->now(); - time_data.period = rclcpp::Duration::from_nanoseconds(0); - time_data.uptime = node_->now(); - time_data_.initRT(time_data); - return JointTrajectoryController::on_activate(state); -} - -controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, - const rclcpp::Duration& /*period*/) -{ - if (state_interfaces_.back().get_name() == "speed_scaling") { - scaling_factor_ = state_interfaces_.back().get_value(); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface not found in hardware interface."); - } - - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - return controller_interface::return_type::OK; - } - - auto resize_joint_trajectory_point = [&](trajectory_msgs::msg::JointTrajectoryPoint& point, size_t size) { - point.positions.resize(size); - if (has_velocity_state_interface_) { - point.velocities.resize(size); - } - if (has_acceleration_state_interface_) { - point.accelerations.resize(size); - } - }; - auto compute_error_for_joint = [&](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current, - const JointTrajectoryPoint& desired) { - // error defined as the difference between current and desired - error.positions[index] = angles::shortest_angular_distance(current.positions[index], desired.positions[index]); - if (has_velocity_state_interface_ && has_velocity_command_interface_) { - error.velocities[index] = desired.velocities[index] - current.velocities[index]; - } - if (has_acceleration_state_interface_ && has_acceleration_command_interface_) { - error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; - } - }; - - // Check if a new external message has been received from nonRT threads - auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) { - fill_partial_goal(*new_external_msg); - sort_to_local_joint_order(*new_external_msg); - traj_external_point_ptr_->update(*new_external_msg); - } - - JointTrajectoryPoint state_current, state_desired, state_error; - const auto joint_num = joint_names_.size(); - resize_joint_trajectory_point(state_current, joint_num); - - // current state update - auto assign_point_from_interface = [&, joint_num](std::vector& trajectory_point_interface, - const auto& joint_inteface) { - for (auto index = 0ul; index < joint_num; ++index) { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); - } - }; - auto assign_interface_from_point = [&, joint_num](auto& joint_inteface, - const std::vector& trajectory_point_interface) { - for (auto index = 0ul; index < joint_num; ++index) { - joint_inteface[index].get().set_value(trajectory_point_interface[index]); - } - }; - - state_current.time_from_start.set__sec(0); - - // Assign values from the hardware - // Position states always exist - assign_point_from_interface(state_current.positions, joint_state_interface_[0]); - // velocity and acceleration states are optional - if (has_velocity_state_interface_) { - assign_point_from_interface(state_current.velocities, joint_state_interface_[1]); - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) { - assign_point_from_interface(state_current.accelerations, joint_state_interface_[2]); - } else { - // Make empty so the property is ignored during interpolation - state_current.accelerations.clear(); - } - } else { - // Make empty so the property is ignored during interpolation - state_current.velocities.clear(); - state_current.accelerations.clear(); - } - - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) { - // Main Speed scaling difference... - // Adjust time with scaling factor - TimeData time_data; - time_data.time = time; - rcl_duration_value_t period = (time_data.time - time_data_.readFromRT()->time).nanoseconds(); - time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * period); - time_data.uptime = time_data_.readFromRT()->uptime + time_data.period; - rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(period); - time_data_.writeFromNonRT(time_data); - - // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(traj_time, state_current); - } - resize_joint_trajectory_point(state_error, joint_num); - - // find segment for current timestamp - joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = - (*traj_point_active_ptr_)->sample(traj_time, state_desired, start_segment_itr, end_segment_itr); - - if (valid_point) { - bool abort = false; - bool outside_goal_tolerance = false; - const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); - - // set values for next hardware write() - if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], state_desired.positions); - } - if (has_velocity_command_interface_) { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); - } - if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); - } - - for (size_t index = 0; index < joint_num; ++index) { - // set values for next hardware write() - compute_error_for_joint(state_error, index, state_current, state_desired); - - if (before_last_point && - !check_state_tolerance_per_joint(state_error, index, default_tolerances_.state_tolerance[index], true)) { - abort = true; - } - // past the final point, check that we end up inside goal tolerance - if (!before_last_point && !check_state_tolerance_per_joint( - state_error, index, default_tolerances_.goal_state_tolerance[index], true)) { - outside_goal_tolerance = true; - } - } - - const auto active_goal = *rt_active_goal_.readFromRT(); - if (active_goal) { - // send feedback - auto feedback = std::make_shared(); - feedback->header.stamp = time; - feedback->joint_names = joint_names_; - - feedback->actual = state_current; - feedback->desired = state_desired; - feedback->error = state_error; - active_goal->setFeedback(feedback); - - // check abort - if (abort || outside_goal_tolerance) { - auto result = std::make_shared(); - - if (abort) { - RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); - } else if (outside_goal_tolerance) { - RCLCPP_WARN(node_->get_logger(), "Aborted due to goal tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - } - active_goal->setAborted(result); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } - - // check goal tolerance - if (!before_last_point) { - if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - - RCLCPP_INFO(node_->get_logger(), "Goal reached, success!"); - } else if (default_tolerances_.goal_time_tolerance != 0.0) { - // if we exceed goal_time_toleralance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - // TODO(anyone): This will break in speed scaling we have to discuss how to handle the goal - // time when the robot scales itself down. - const double difference = time.seconds() - traj_end.seconds(); - if (difference > default_tolerances_.goal_time_tolerance) { - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - active_goal->setAborted(result); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_WARN(node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", difference); - } - } - } - } - } - } - - publish_state(state_desired, state_current, state_error); - return controller_interface::return_type::OK; -} - -} // namespace ur_controllers - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(ur_controllers::ScaledJointTrajectoryController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/speed_scaling_state_broadcaster.cpp b/ur_controllers/src/speed_scaling_state_broadcaster.cpp deleted file mode 100644 index 773f2f9b2..000000000 --- a/ur_controllers/src/speed_scaling_state_broadcaster.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright 2019, FZI Forschungszentrum Informatik -// -// 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. - -//---------------------------------------------------------------------- -/*!\file - * - * \author Marvin Große Besselmann grosse@fzi.de - * \date 2021-02-10 - * - */ -//---------------------------------------------------------------------- - -#include "ur_controllers/speed_scaling_state_broadcaster.hpp" - -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rcpputils/split.hpp" -#include "rcutils/logging_macros.h" - -namespace rclcpp_lifecycle -{ -class State; -} // namespace rclcpp_lifecycle - -namespace ur_controllers -{ -SpeedScalingStateBroadcaster::SpeedScalingStateBroadcaster() -{ -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SpeedScalingStateBroadcaster::on_init() -{ - try { - auto_declare("state_publish_rate", 100.0); - } catch (std::exception& e) { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration SpeedScalingStateBroadcaster::command_interface_configuration() const -{ - return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE }; -} - -controller_interface::InterfaceConfiguration SpeedScalingStateBroadcaster::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - config.names.push_back("speed_scaling/speed_scaling_factor"); - return config; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -SpeedScalingStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) -{ - if (!node_->get_parameter("state_publish_rate", publish_rate_)) { - RCLCPP_INFO(get_node()->get_logger(), "Parameter 'state_publish_rate' not set"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } else { - RCLCPP_INFO(get_node()->get_logger(), "Publisher rate set to : %.1f Hz", publish_rate_); - } - - try { - speed_scaling_state_publisher_ = - get_node()->create_publisher("~/speed_scaling", rclcpp::SystemDefaultsQoS()); - } catch (const std::exception& e) { - // get_node() may throw, logging raw here - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -SpeedScalingStateBroadcaster::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -SpeedScalingStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type SpeedScalingStateBroadcaster::update(const rclcpp::Time& /*time*/, - const rclcpp::Duration& period) -{ - if (publish_rate_ > 0.0 && period > rclcpp::Duration(1.0 / publish_rate_, 0.0)) { - // Speed scaling is the only interface of the controller - speed_scaling_state_msg_.data = state_interfaces_[0].get_value() * 100.0; - - // publish - speed_scaling_state_publisher_->publish(speed_scaling_state_msg_); - } - return controller_interface::return_type::OK; -} - -} // namespace ur_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS(ur_controllers::SpeedScalingStateBroadcaster, controller_interface::ControllerInterface)