Merge in changes

srdf/include/srdf/model.h

@@ -135,25 +135,6 @@ public:
     /// The name of the group that includes the joints & links this end effector consists of
     std::string component_group_;
   };
-
-  /// Representation of a visual sensor
-  struct VisualSensor
-  {
-    /// The name of the sensor
-    std::string name_;
-    
-    /// The reference frame for the data produced by the sensor, such that the sensor looks in the direction of the z axis and the optical centre is at the origin of the sensor frame
-    std::string frame_;
-    
-    /// The field of view angle with respect to the z axis (radians)
-    double      fov_angle_;
-    
-    /// The minimum distance along the z axis for sensor data to be considered valid
-    double      min_range_;
-
-    /// The maximum distance along the z axis for sensor data to be considered valid
-    double      max_range_;
-  };
   
   /// A named state for a particular group
   struct GroupState
@@ -230,7 +211,6 @@ private:
   void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
   void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
   void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
-  void loadVisualSensors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
   void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
 
   std::string                    name_;
@@ -238,7 +218,6 @@ private:
   std::vector<GroupState>        group_states_;
   std::vector<VirtualJoint>      virtual_joints_;
   std::vector<EndEffector>       end_effectors_;
-  std::vector<VisualSensor>      visual_sensors_;
   std::vector<DisabledCollision> disabled_collisions_;
 };
 

srdf/src/model.cpp

@@ -351,74 +351,6 @@ void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlE
   }
 }
 
-void srdf::Model::loadVisualSensors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
-{
-  for (TiXmlElement* s_xml = robot_xml->FirstChildElement("visual_sensor"); s_xml; s_xml = s_xml->NextSiblingElement("visual_sensor"))
-  {
-    const char *sname = s_xml->Attribute("name");
-    const char *frame = s_xml->Attribute("frame");
-    const char *fov_angle = s_xml->Attribute("fov_angle");
-    const char *min_range = s_xml->Attribute("min_range");
-    const char *max_range = s_xml->Attribute("max_range");
-    if (!sname)
-    {
-      ROS_ERROR("Name of visual sensor is not specified");
-      continue;
-    }
-    if (!frame)
-    {
-      ROS_ERROR("No frame specified for visual sensor '%s'", sname);
-      continue;
-    }
-    if (!fov_angle)
-    {
-      ROS_ERROR("No field of view angle specified for visual sensor '%s'", sname);
-      continue;
-    }
-    if (!min_range)
-    {
-      ROS_ERROR("No minimum range along Z axis specified for visual sensor '%s'", sname);
-      continue;
-    }
-    if (!max_range)
-    {
-      ROS_ERROR("No maximum range along Z axis specified for visual sensor '%s'", sname);
-      continue;
-    }
-    VisualSensor s;
-    s.name_ = std::string(sname); boost::trim(s.name_);
-    s.frame_ = std::string(frame); boost::trim(s.frame_);
-    try
-    {
-      s.fov_angle_ = boost::lexical_cast<double>(std::string(fov_angle));
-    }
-    catch (boost::bad_lexical_cast &e)
-    {     
-      ROS_ERROR("Unable to parse field of view angle ('%s') for sensor '%s'", fov_angle, sname);
-      continue;
-    }
-    try
-    {
-      s.min_range_ = boost::lexical_cast<double>(std::string(min_range));
-    }
-    catch (boost::bad_lexical_cast &e)
-    {     
-      ROS_ERROR("Unable to parse minimum range ('%s') for sensor '%s'", min_range, sname);
-      continue;
-    }
-    try
-    {
-      s.max_range_ = boost::lexical_cast<double>(std::string(max_range));
-    }
-    catch (boost::bad_lexical_cast &e)
-    {     
-      ROS_ERROR("Unable to parse maximum range ('%s') for sensor '%s'", max_range, sname);
-      continue;
-    }
-    visual_sensors_.push_back(s);
-  }
-}
-
 void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
 {
   for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector"))
@@ -521,7 +453,6 @@ bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *
   loadGroups(urdf_model, robot_xml);
   loadGroupStates(urdf_model, robot_xml);
   loadEndEffectors(urdf_model, robot_xml); 
-  loadVisualSensors(urdf_model, robot_xml);
   loadDisabledCollisions(urdf_model, robot_xml);
   
   return true;
@@ -577,7 +508,6 @@ void srdf::Model::clear(void)
   group_states_.clear();
   virtual_joints_.clear();
   end_effectors_.clear();
-  visual_sensors_.clear();
   disabled_collisions_.clear();
 }