added visual sensors to SRDF

srdf/CMakeLists.txt

@@ -17,14 +17,14 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(TXML REQUIRED tinyxml)
+include_directories(${TXML_INCLUDE_DIRS})
+link_directories(${TXML_LIBRARY_DIRS})
 
 #common commands for building c++ executables and libraries
 rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
-target_link_libraries(${PROJECT_NAME} tinyxml)
+target_link_libraries(${PROJECT_NAME} ${TXML_LIBRARIES})
 
 rosbuild_add_gtest(test_parser test/test_parser.cpp)
 target_link_libraries(test_parser ${PROJECT_NAME})

srdf/include/srdf/model.h

@@ -135,6 +135,25 @@ 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
+    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
@@ -194,13 +213,15 @@ 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_;
   std::vector<Group>                                groups_;
   std::vector<GroupState>                           group_states_;
   std::vector<VirtualJoint>                         virtual_joints_;
   std::vector<EndEffector>                          end_effectors_;
+  std::vector<VisualSensor>                         visual_sensors_;
   std::vector<std::pair<std::string, std::string> > disabled_collisions_;
 };
 

srdf/src/model.cpp

@@ -351,6 +351,74 @@ 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"))
@@ -448,7 +516,8 @@ bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *
   loadVirtualJoints(urdf_model, robot_xml);
   loadGroups(urdf_model, robot_xml);
   loadGroupStates(urdf_model, robot_xml);
-  loadEndEffectors(urdf_model, robot_xml);
+  loadEndEffectors(urdf_model, robot_xml); 
+  loadVisualSensors(urdf_model, robot_xml);
   loadDisabledCollisions(urdf_model, robot_xml);
   
   return true;
@@ -504,5 +573,6 @@ void srdf::Model::clear(void)
   group_states_.clear();
   virtual_joints_.clear();
   end_effectors_.clear();
+  visual_sensors_.clear();
   disabled_collisions_.clear();
 }