|
19 | 19 | //---------------------------------------------------------------------- |
20 | 20 | /*!\file |
21 | 21 | * |
22 | | - * \author Felix Exner exner@fzi.de |
23 | | - * \date 2019-04-17 |
| 22 | + * \author Marvin Große Besselmann grosse@fzi.de |
| 23 | + * \date 2021-02-10 |
24 | 24 | * |
25 | 25 | */ |
26 | 26 | //---------------------------------------------------------------------- |
27 | 27 |
|
28 | 28 | #include "ur_controllers/speed_scaling_state_controller.h" |
| 29 | + |
| 30 | +#include <stddef.h> |
| 31 | +#include <limits> |
| 32 | +#include <memory> |
| 33 | +#include <string> |
| 34 | +#include <unordered_map> |
| 35 | +#include <vector> |
| 36 | + |
| 37 | +#include "hardware_interface/types/hardware_interface_return_values.hpp" |
| 38 | +#include "hardware_interface/types/hardware_interface_type_values.hpp" |
| 39 | +#include "rclcpp/clock.hpp" |
| 40 | +#include "rclcpp/qos.hpp" |
| 41 | +#include "rclcpp/qos_event.hpp" |
| 42 | +#include "rclcpp/time.hpp" |
| 43 | +#include "rclcpp_lifecycle/lifecycle_node.hpp" |
| 44 | +#include "rcpputils/split.hpp" |
| 45 | +#include "rcutils/logging_macros.h" |
| 46 | +#include "std_msgs/msg/header.hpp" |
| 47 | + |
| 48 | +namespace rclcpp_lifecycle |
| 49 | +{ |
| 50 | +class State; |
| 51 | +} // namespace rclcpp_lifecycle |
| 52 | + |
| 53 | +namespace ur_controllers |
| 54 | +{ |
| 55 | +const auto kUninitializedValue = std::numeric_limits<double>::quiet_NaN(); |
| 56 | +using hardware_interface::HW_IF_POSITION; |
| 57 | +using hardware_interface::HW_IF_VELOCITY; |
| 58 | +using hardware_interface::HW_IF_EFFORT; |
| 59 | + |
| 60 | + |
| 61 | +SpeedScalingStateController::SpeedScalingStateController() |
| 62 | +{} |
| 63 | + |
| 64 | +controller_interface::InterfaceConfiguration SpeedScalingStateController::command_interface_configuration() |
| 65 | +const |
| 66 | +{ |
| 67 | + return controller_interface::InterfaceConfiguration{controller_interface:: |
| 68 | + interface_configuration_type::NONE}; |
| 69 | +} |
| 70 | + |
| 71 | +controller_interface::InterfaceConfiguration SpeedScalingStateController::state_interface_configuration() |
| 72 | +const |
| 73 | +{ |
| 74 | + return controller_interface::InterfaceConfiguration{controller_interface:: |
| 75 | + interface_configuration_type::ALL}; |
| 76 | +} |
| 77 | + |
| 78 | +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn |
| 79 | +SpeedScalingStateController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) |
| 80 | +{ |
| 81 | + try { |
| 82 | + // TODO change subscriber to single value |
| 83 | + joint_state_publisher_ = get_node()->create_publisher<sensor_msgs::msg::JointState>( |
| 84 | + "joint_states", rclcpp::SystemDefaultsQoS()); |
| 85 | + |
| 86 | + } catch (const std::exception & e) { |
| 87 | + // get_node() may throw, logging raw here |
| 88 | + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); |
| 89 | + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; |
| 90 | + } |
| 91 | + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; |
| 92 | +} |
| 93 | + |
| 94 | +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn |
| 95 | +SpeedScalingStateController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) |
| 96 | +{ |
| 97 | + if (!init_joint_data()) { |
| 98 | + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; |
| 99 | + } |
| 100 | + |
| 101 | + init_joint_state_msg(); |
| 102 | + |
| 103 | + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; |
| 104 | +} |
| 105 | + |
| 106 | +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn |
| 107 | +SpeedScalingStateController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) |
| 108 | +{ |
| 109 | + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; |
| 110 | +} |
| 111 | + |
| 112 | +template<typename T> |
| 113 | +bool has_any_key( |
| 114 | + const std::unordered_map<std::string, T> & map, |
| 115 | + const std::vector<std::string> & keys) |
| 116 | +{ |
| 117 | + bool found_key = false; |
| 118 | + for (const auto & key_item : map) { |
| 119 | + const auto & key = key_item.first; |
| 120 | + if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend()) { |
| 121 | + found_key = true; |
| 122 | + break; |
| 123 | + } |
| 124 | + } |
| 125 | + return found_key; |
| 126 | +} |
| 127 | + |
| 128 | +bool SpeedScalingStateController::init_joint_data() |
| 129 | +{ |
| 130 | + // loop in reverse order, this maintains the order of values at retrieval time |
| 131 | + for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) { |
| 132 | + // initialize map if name is new |
| 133 | + if (name_if_value_mapping_.count(si->get_name()) == 0) { |
| 134 | + name_if_value_mapping_[si->get_name()] = {}; |
| 135 | + } |
| 136 | + // add interface name |
| 137 | + name_if_value_mapping_[si->get_name()][si->get_interface_name()] = |
| 138 | + kUninitializedValue; |
| 139 | + } |
| 140 | + |
| 141 | + // filter state interfaces that have a speed scaling factor |
| 142 | + // the rest will be ignored for this message |
| 143 | + for (const auto & name_ifv : name_if_value_mapping_) { |
| 144 | + const auto & interfaces_and_values = name_ifv.second; |
| 145 | + if (has_any_key(interfaces_and_values, {"speed_scaling_factor"})) { |
| 146 | + joint_names_.push_back(name_ifv.first); |
| 147 | + } |
| 148 | + } |
| 149 | + |
| 150 | + return true; |
| 151 | +} |
| 152 | + |
| 153 | +void SpeedScalingStateController::init_joint_state_msg() |
| 154 | +{ |
| 155 | + const size_t num_joints = joint_names_.size(); |
| 156 | + |
| 157 | + /// @note joint_state_msg publishes position, velocity and effort for all joints, |
| 158 | + /// with at least one of these interfaces, the rest are omitted from this message |
| 159 | + |
| 160 | + // default initialization for joint state message |
| 161 | + joint_state_msg_.name = joint_names_; |
| 162 | + joint_state_msg_.position.resize(num_joints, kUninitializedValue); |
| 163 | + joint_state_msg_.velocity.resize(num_joints, kUninitializedValue); |
| 164 | + joint_state_msg_.effort.resize(num_joints, kUninitializedValue); |
| 165 | +} |
| 166 | + |
| 167 | +double get_value( |
| 168 | + const std::unordered_map<std::string, std::unordered_map<std::string, double>> & map, |
| 169 | + const std::string & name, const std::string & interface_name) |
| 170 | +{ |
| 171 | + const auto & interfaces_and_values = map.at(name); |
| 172 | + const auto interface_and_value = interfaces_and_values.find(interface_name); |
| 173 | + if (interface_and_value != interfaces_and_values.cend()) { |
| 174 | + return interface_and_value->second; |
| 175 | + } else { |
| 176 | + return kUninitializedValue; |
| 177 | + } |
| 178 | +} |
| 179 | + |
| 180 | +controller_interface::return_type |
| 181 | +SpeedScalingStateController::update() |
| 182 | +{ |
| 183 | + for (const auto & state_interface : state_interfaces_) { |
| 184 | + name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] = |
| 185 | + state_interface.get_value(); |
| 186 | + RCLCPP_DEBUG( |
| 187 | + get_node()->get_logger(), "%s/%s: %f\n", |
| 188 | + state_interface.get_name().c_str(), |
| 189 | + state_interface.get_interface_name().c_str(), |
| 190 | + state_interface.get_value()); |
| 191 | + } |
| 192 | + |
| 193 | + joint_state_msg_.header.stamp = node_->get_clock()->now(); |
| 194 | + |
| 195 | + for (auto i = 0ul; i < joint_names_.size(); ++i) { |
| 196 | + std::cout << get_value(name_if_value_mapping_, joint_names_[i], "speed_scaling_factor") << std::endl; |
| 197 | + } |
| 198 | + |
| 199 | + // publish |
| 200 | + joint_state_publisher_->publish(joint_state_msg_); |
| 201 | + |
| 202 | + return controller_interface::return_type::SUCCESS; |
| 203 | +} |
| 204 | + |
| 205 | +} // namespace joint_state_controller |
| 206 | + |
| 207 | +#include "pluginlib/class_list_macros.hpp" |
| 208 | + |
| 209 | +PLUGINLIB_EXPORT_CLASS( |
| 210 | + ur_controllers::SpeedScalingStateController, controller_interface::ControllerInterface) |
0 commit comments