motion_control_handle.cpp
1 //////////////////////////////////////////////////////////////////////////////// 2 // Copyright 2019 FZI Research Center for Information Technology 3 // 4 // Redistribution and use in source and binary forms, with or without 5 // modification, are permitted provided that the following conditions are met: 6 // 7 // 1. Redistributions of source code must retain the above copyright notice, 8 // this list of conditions and the following disclaimer. 9 // 10 // 2. Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // 14 // 3. Neither the name of the copyright holder nor the names of its 15 // contributors may be used to endorse or promote products derived from this 16 // software without specific prior written permission. 17 // 18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 // POSSIBILITY OF SUCH DAMAGE. 29 //////////////////////////////////////////////////////////////////////////////// 30 31 //----------------------------------------------------------------------------- 32 /*!\file motion_control_handle.cpp 33 * 34 * \author Stefan Scherzinger <scherzin@fzi.de> 35 * \date 2018/06/20 36 * 37 */ 38 //----------------------------------------------------------------------------- 39 40 #include <cartesian_controller_handles/motion_control_handle.h> 41 #include <urdf/model.h> 42 43 #include <kdl/tree.hpp> 44 #include <kdl_parser/kdl_parser.hpp> 45 46 #include "controller_interface/helpers.hpp" 47 #include "hardware_interface/types/hardware_interface_type_values.hpp" 48 #include "rclcpp/node.hpp" 49 #include "rclcpp/time.hpp" 50 #include "visualization_msgs/msg/detail/interactive_marker_feedback__struct.hpp" 51 52 namespace cartesian_controller_handles 53 { 54 MotionControlHandle::MotionControlHandle() {} 55 56 MotionControlHandle::~MotionControlHandle() {} 57 58 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 59 MotionControlHandle::on_activate(const rclcpp_lifecycle::State & previous_state) 60 { 61 // Get state handles. 62 if (!controller_interface::get_ordered_interfaces( 63 state_interfaces_, m_joint_names, hardware_interface::HW_IF_POSITION, m_joint_handles)) 64 { 65 RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", 66 m_joint_names.size(), hardware_interface::HW_IF_POSITION, m_joint_handles.size()); 67 return CallbackReturn::ERROR; 68 } 69 70 m_current_pose = getEndEffectorPose(); 71 m_server->setPose(m_marker.name, m_current_pose.pose); 72 m_server->applyChanges(); 73 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 74 } 75 76 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 77 MotionControlHandle::on_deactivate(const rclcpp_lifecycle::State & previous_state) 78 { 79 m_joint_handles.clear(); 80 this->release_interfaces(); 81 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 82 } 83 84 controller_interface::return_type MotionControlHandle::update(const rclcpp::Time & time, 85 const rclcpp::Duration & period) 86 { 87 if (m_current_pose.header.frame_id != m_robot_base_link && m_current_pose.header.frame_id != "") 88 { 89 RCLCPP_ERROR( 90 get_node()->get_logger(), 91 "The reference frame \"%s\" selected in Rviz is not a fixed frame. Please select a " 92 "fixed frame.", 93 m_current_pose.header.frame_id.c_str()); 94 return controller_interface::return_type::ERROR; 95 } 96 // Publish marker pose 97 m_current_pose.header.stamp = get_node()->now(); 98 m_current_pose.header.frame_id = m_robot_base_link; 99 m_pose_publisher->publish(m_current_pose); 100 m_server->applyChanges(); 101 102 return controller_interface::return_type::OK; 103 } 104 105 controller_interface::InterfaceConfiguration MotionControlHandle::command_interface_configuration() 106 const 107 { 108 controller_interface::InterfaceConfiguration conf; 109 conf.type = controller_interface::interface_configuration_type::NONE; 110 return conf; 111 } 112 113 controller_interface::InterfaceConfiguration MotionControlHandle::state_interface_configuration() 114 const 115 { 116 controller_interface::InterfaceConfiguration conf; 117 conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; 118 conf.names.reserve(m_joint_names.size()); // Only position 119 for (const auto & joint_name : m_joint_names) 120 { 121 conf.names.push_back(joint_name + "/position"); 122 } 123 return conf; 124 } 125 126 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 127 MotionControlHandle::on_init() 128 { 129 auto_declare<std::string>("robot_description", ""); 130 auto_declare<std::string>("robot_base_link", ""); 131 auto_declare<std::string>("end_effector_link", ""); 132 auto_declare<std::string>("controller_name", ""); 133 auto_declare<std::vector<std::string> >("joints", std::vector<std::string>()); 134 135 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 136 } 137 138 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 139 MotionControlHandle::on_configure(const rclcpp_lifecycle::State & previous_state) 140 { 141 // Get kinematics specific configuration 142 urdf::Model robot_model; 143 KDL::Tree robot_tree; 144 145 #if defined CARTESIAN_CONTROLLERS_JAZZY 146 std::string robot_description = this->get_robot_description(); 147 #else 148 std::string robot_description = get_node()->get_parameter("robot_description").as_string(); 149 #endif 150 if (robot_description.empty()) 151 { 152 RCLCPP_ERROR(get_node()->get_logger(), "robot_description is empty"); 153 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 154 } 155 m_robot_base_link = get_node()->get_parameter("robot_base_link").as_string(); 156 if (m_robot_base_link.empty()) 157 { 158 RCLCPP_ERROR(get_node()->get_logger(), "robot_base_link is empty"); 159 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 160 } 161 m_end_effector_link = get_node()->get_parameter("end_effector_link").as_string(); 162 if (m_end_effector_link.empty()) 163 { 164 RCLCPP_ERROR(get_node()->get_logger(), "end_effector_link is empty"); 165 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 166 } 167 168 // Build a kinematic chain of the robot 169 if (!robot_model.initString(robot_description)) 170 { 171 RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf model from 'robot_description'"); 172 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 173 } 174 if (!kdl_parser::treeFromUrdfModel(robot_model, robot_tree)) 175 { 176 RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from urdf model"); 177 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 178 } 179 if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, m_robot_chain)) 180 { 181 const std::string error = 182 "" 183 "Failed to parse robot chain from urdf model. " 184 "Do robot_base_link and end_effector_link exist?"; 185 RCLCPP_ERROR(get_node()->get_logger(), "%s", error.c_str()); 186 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 187 } 188 189 // Get names of the joints 190 m_joint_names = get_node()->get_parameter("joints").as_string_array(); 191 if (m_joint_names.empty()) 192 { 193 RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty"); 194 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 195 } 196 197 // Publishers 198 if (get_node()->get_parameter("controller_name").as_string().empty()) 199 { 200 m_pose_publisher = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>( 201 get_node()->get_name() + std::string("/target_frame"), 10); 202 } 203 else 204 { 205 m_pose_publisher = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>( 206 get_node()->get_parameter("controller_name").as_string() + 207 std::string("/target_frame"), 208 10); 209 } 210 211 // Initialize kinematics 212 m_fk_solver.reset(new KDL::ChainFkSolverPos_recursive(m_robot_chain)); 213 m_current_pose = getEndEffectorPose(); 214 215 // Configure the interactive marker for usage in RViz 216 m_server.reset( 217 new interactive_markers::InteractiveMarkerServer(get_node()->get_name(), get_node())); 218 m_marker.header.frame_id = m_robot_base_link; 219 m_marker.header.stamp = get_node()->now(); 220 m_marker.scale = 0.1; 221 m_marker.name = "motion_control_handle"; 222 m_marker.pose = m_current_pose.pose; 223 m_marker.description = "6D control of link: " + m_end_effector_link; 224 225 prepareMarkerControls(m_marker); 226 227 // Add the interactive marker to the server 228 m_server->insert(m_marker); 229 230 // Add callback for motion in RViz 231 m_server->setCallback( 232 m_marker.name, 233 std::bind(&MotionControlHandle::updateMotionControlCallback, this, std::placeholders::_1), 234 visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE); 235 236 // Add callback for menu interaction in RViz 237 m_server->setCallback( 238 m_marker.name, 239 std::bind(&MotionControlHandle::updateMarkerMenuCallback, this, std::placeholders::_1), 240 visualization_msgs::msg::InteractiveMarkerFeedback::MENU_SELECT); 241 242 // Activate configuration 243 m_server->applyChanges(); 244 245 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 246 } 247 248 void MotionControlHandle::updateMotionControlCallback( 249 const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) 250 { 251 // Move marker in RViz 252 m_server->setPose(feedback->marker_name, feedback->pose, feedback->header); 253 m_server->applyChanges(); 254 255 // Store for later broadcasting 256 m_current_pose.pose = feedback->pose; 257 m_current_pose.header.frame_id = feedback->header.frame_id; 258 m_current_pose.header.stamp = feedback->header.stamp; 259 } 260 261 void MotionControlHandle::updateMarkerMenuCallback( 262 const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) 263 { 264 } 265 266 void MotionControlHandle::prepareMarkerControls(visualization_msgs::msg::InteractiveMarker & marker) 267 { 268 // Add colored sphere as visualization 269 constexpr double marker_scale = 0.05; 270 addMarkerVisualization(marker, marker_scale); 271 272 // Create move and rotate controls along all axis 273 addAxisControl(marker, 1, 0, 0); 274 addAxisControl(marker, 0, 1, 0); 275 addAxisControl(marker, 0, 0, 1); 276 } 277 278 void MotionControlHandle::addMarkerVisualization( 279 visualization_msgs::msg::InteractiveMarker & marker, double scale) 280 { 281 // Create a sphere as a handle 282 visualization_msgs::msg::Marker visual; 283 visual.type = visualization_msgs::msg::Marker::SPHERE; 284 visual.scale.x = scale; // bounding box in meter 285 visual.scale.y = scale; 286 visual.scale.z = scale; 287 visual.color.r = 1.0; 288 visual.color.g = 0.5; 289 visual.color.b = 0.0; 290 visual.color.a = 1.0; 291 292 // Create a non-interactive control for the appearance 293 visualization_msgs::msg::InteractiveMarkerControl visual_control; 294 visual_control.always_visible = true; 295 visual_control.markers.push_back(visual); 296 marker.controls.push_back(visual_control); 297 } 298 299 void MotionControlHandle::addAxisControl(visualization_msgs::msg::InteractiveMarker & marker, 300 double x, double y, double z) 301 { 302 if (x == 0 && y == 0 && z == 0) 303 { 304 return; 305 } 306 307 visualization_msgs::msg::InteractiveMarkerControl control; 308 309 double norm = std::sqrt(1 + x * x + y * y + z * z); 310 control.orientation.w = 1 / norm; 311 control.orientation.x = x / norm; 312 control.orientation.y = y / norm; 313 control.orientation.z = z / norm; 314 315 control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; 316 marker.controls.push_back(control); 317 318 control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; 319 marker.controls.push_back(control); 320 } 321 322 geometry_msgs::msg::PoseStamped MotionControlHandle::getEndEffectorPose() 323 { 324 KDL::JntArray positions(m_joint_handles.size()); 325 for (size_t i = 0; i < m_joint_handles.size(); ++i) 326 { 327 positions(i) = m_joint_handles[i].get().get_value(); 328 } 329 330 KDL::Frame tmp; 331 m_fk_solver->JntToCart(positions, tmp); 332 333 geometry_msgs::msg::PoseStamped current; 334 current.pose.position.x = tmp.p.x(); 335 current.pose.position.y = tmp.p.y(); 336 current.pose.position.z = tmp.p.z(); 337 tmp.M.GetQuaternion(current.pose.orientation.x, current.pose.orientation.y, 338 current.pose.orientation.z, current.pose.orientation.w); 339 340 return current; 341 } 342 343 } // namespace cartesian_controller_handles 344 345 // Pluginlib 346 #include <pluginlib/class_list_macros.hpp> 347 348 PLUGINLIB_EXPORT_CLASS(cartesian_controller_handles::MotionControlHandle, 349 controller_interface::ControllerInterface)