loads URDF model from parameter server

Author: hsd-dev

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -45,6 +45,7 @@
 #include <ur_msgs/SetIO.h>
 #include "ur_msgs/SetSpeedSliderFraction.h"
 
+#include <urdf/model.h>
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
 
@@ -156,6 +157,13 @@ class HardwareInterface : public hardware_interface::RobotHW
   bool shouldResetControllers();
 
 protected:
+
+  /*!
+   * \brief Loads URDF model from robot_description parameter
+   *
+   * Requires robot_description paramter to be set on the parameter server
+   */
+  void loadURDF(ros::NodeHandle& nh, std::string param_name);
   /*!
    * \brief Transforms force-torque measurements reported from the robot from base to tool frame.
    *
@@ -223,6 +231,9 @@ class HardwareInterface : public hardware_interface::RobotHW
   ur_controllers::ScaledVelocityJointInterface svj_interface_;
   hardware_interface::ForceTorqueSensorInterface fts_interface_;
 
+
+  urdf::Model* urdf_model_;
+
   vector6d_t joint_position_command_;
   vector6d_t joint_velocity_command_;
   vector6d_t joint_positions_;

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -72,6 +72,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   std::string output_recipe_filename;
   std::string input_recipe_filename;
 
+  // Load URDF file from parameter server
+  loadURDF(robot_hw_nh, "robot_description");
+
   // The robot's IP address.
   if (!robot_hw_nh.getParam("robot_ip", robot_ip_))
   {
@@ -956,6 +959,37 @@ bool HardwareInterface::checkControllerClaims(const std::set<std::string>& claim
   }
   return false;
 }
+
+void HardwareInterface::loadURDF(ros::NodeHandle& nh, std::string param_name)
+{
+  std::string urdf_string;
+  urdf_model_ = new urdf::Model();
+
+  // search and wait for robot_description on param server
+  while (urdf_string.empty() && ros::ok())
+  {
+    std::string search_param_name;
+    if (nh.searchParam(param_name, search_param_name))
+    {
+      ROS_INFO_STREAM("Waiting for model URDF on the ROS param server at location: " << nh.getNamespace()
+                                                                                     << search_param_name);
+      nh.getParam(search_param_name, urdf_string);
+    }
+    else
+    {
+      ROS_INFO_STREAM("Waiting for model URDF on the ROS param server at location: " << nh.getNamespace()
+                                                                                     << param_name);
+      nh.getParam(param_name, urdf_string);
+    }
+
+    usleep(100000);
+  }
+
+  if (!urdf_model_->initString(urdf_string))
+    ROS_ERROR_STREAM("Unable to load URDF model");
+  else
+    ROS_DEBUG_STREAM("Received URDF from param server");
+}
 }  // namespace ur_driver
 
 PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)