diff --git a/srdf/CMakeLists.txt b/srdf/CMakeLists.txt
index 098d672..14a7449 100644
--- a/srdf/CMakeLists.txt
+++ b/srdf/CMakeLists.txt
@@ -24,3 +24,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
target_link_libraries(${PROJECT_NAME} tinyxml)
+
+rosbuild_add_gtest(test_parser test/test_parser.cpp)
+target_link_libraries(test_parser ${PROJECT_NAME})
diff --git a/srdf/manifest.xml b/srdf/manifest.xml
index 029eff8..d5180b3 100644
--- a/srdf/manifest.xml
+++ b/srdf/manifest.xml
@@ -12,7 +12,7 @@
-
+
diff --git a/srdf/src/model.cpp b/srdf/src/model.cpp
index 857a250..a70c99c 100644
--- a/srdf/src/model.cpp
+++ b/srdf/src/model.cpp
@@ -36,6 +36,7 @@
#include "srdf/model.h"
#include
+#include
#include
#include
#include
@@ -74,10 +75,16 @@ void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXm
continue;
}
VirtualJoint vj;
+ vj.type_ = std::string(type);
+ std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower);
+ if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed")
+ {
+ ROS_ERROR("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type);
+ vj.type_ = "fixed";
+ }
vj.name_ = std::string(jname);
vj.child_link_ = std::string(child);
vj.parent_frame_ = std::string(parent);
- vj.type_ = std::string(type);
virtual_joints_.push_back(vj);
}
}
@@ -194,6 +201,8 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
}
g.subgroups_.push_back(std::string(sub));
}
+ if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
+ ROS_WARN("Group '%s' is empty.", gname);
groups_.push_back(g);
}
@@ -307,9 +316,9 @@ void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlE
}
std::string jval_str = std::string(jval);
std::stringstream ss(jval_str);
- while (!ss.eof())
+ while (ss.good() && !ss.eof())
{
- double val; ss >> val;
+ double val; ss >> val >> std::ws;
gs.joint_values_[jname_str].push_back(val);
}
if (gs.joint_values_.empty())
diff --git a/srdf/test/res/pr2_desc.1.srdf b/srdf/test/res/pr2_desc.1.srdf
new file mode 100644
index 0000000..5f11ab9
--- /dev/null
+++ b/srdf/test/res/pr2_desc.1.srdf
@@ -0,0 +1,3 @@
+
+
+
diff --git a/srdf/test/res/pr2_desc.2.srdf b/srdf/test/res/pr2_desc.2.srdf
new file mode 100644
index 0000000..06c3fd7
--- /dev/null
+++ b/srdf/test/res/pr2_desc.2.srdf
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
diff --git a/srdf/test/res/pr2_desc.3.srdf b/srdf/test/res/pr2_desc.3.srdf
new file mode 100644
index 0000000..33208de
--- /dev/null
+++ b/srdf/test/res/pr2_desc.3.srdf
@@ -0,0 +1,72 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/srdf/test/res/pr2_desc.urdf b/srdf/test/res/pr2_desc.urdf
new file mode 100644
index 0000000..cbc5c77
--- /dev/null
+++ b/srdf/test/res/pr2_desc.urdf
@@ -0,0 +1,3238 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ 1000.0
+
+
+
+ true
+ 1.0
+ 5
+
+ power_state
+ 10.0
+ 87.78
+ -474
+ 525
+ 15.52
+ 16.41
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640
+ 640
+ 1
+ 0.0 0.0 0.0
+ false
+ -129.998394137
+ 129.998394137
+ 0.08
+ 10.0
+ 0.01
+ 20
+
+ 0.005
+ true
+ 20
+ base_scan
+ base_laser_link
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 79.2380952381
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -79.2380952381
+
+
+
+
+
+ true
+
+ base_link_geom
+ 100.0
+
+ true
+ 100.0
+ base_bumper
+
+
+
+
+
+
+ true
+ 100.0
+ base_link
+ base_pose_ground_truth
+ 0.01
+ map
+ 25.7 25.7 0
+
+ 0 0 0
+
+
+ base_footprint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ torso_lift_link_geom
+ 100.0
+
+ true
+ 100.0
+ torso_lift_bumper
+
+
+
+
+
+
+
+ -52143.33
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ 100.0
+ imu_link
+ torso_lift_imu/data
+ 2.89e-08
+ 0 0 0
+ 0 0 0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 6.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 6.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ R8G8B8
+ 2448 2050
+ 45
+ 0.1
+ 100
+ 20.0
+
+ true
+ 20.0
+ /prosilica/image_raw
+ /prosilica/camera_info
+ /prosilica/request_image
+ high_def_frame
+ 1224.5
+ 1224.5
+ 1025.5
+ 2955
+
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640 480
+ BAYER_BGGR8
+ 90
+ 0.1
+ 100
+ 25.0
+
+ true
+ 25.0
+ wide_stereo/left/image_raw
+ wide_stereo/left/camera_info
+ wide_stereo_optical_frame
+ 0
+ 320.5
+ 320.5
+ 240.5
+
+
+ 320
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+
+
+
+ true
+ PR2/Blue
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640 480
+ BAYER_BGGR8
+ 90
+ 0.1
+ 100
+ 25.0
+
+ true
+ 25.0
+ wide_stereo/right/image_raw
+ wide_stereo/right/camera_info
+ wide_stereo_optical_frame
+ 0.09
+ 320.5
+ 320.5
+ 240.5
+
+
+ 320
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+
+
+
+ true
+ PR2/Blue
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640 480
+ L8
+ 45
+ 0.1
+ 100
+ 25.0
+
+ true
+ 25.0
+ narrow_stereo/left/image_raw
+ narrow_stereo/left/camera_info
+ narrow_stereo_optical_frame
+ 0
+ 320.5
+ 320.5
+ 240.5
+
+
+ 772.55
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+
+
+
+ true
+ PR2/Blue
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640 480
+ L8
+ 45
+ 0.1
+ 100
+ 25.0
+
+ true
+ 25.0
+ narrow_stereo/right/image_raw
+ narrow_stereo/right/camera_info
+ narrow_stereo_optical_frame
+ 0.09
+ 320.5
+ 320.5
+ 240.5
+
+
+ 772.55
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+
+
+
+ true
+ PR2/Blue
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ 15.0
+ stereo_projection_pattern_high_res_red.png
+ projector_wg6802418_child_frame
+ stereo_projection_pattern_filter.png
+ projector_wg6802418_controller/image
+ projector_wg6802418_controller/projector
+ 0.785398163397
+ 0.4
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640
+ 640
+ 1
+ 0.0 0.0 0.0
+ false
+ -79.9999999086
+ 79.9999999086
+ 0.08
+ 10.0
+ 0.01
+ 40
+
+ 0.005
+ true
+ 40
+ tilt_scan
+ laser_tilt_link
+
+
+
+
+
+
+
+
+
+ -6.05
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+ 32.6525111499
+
+
+ true
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+ 63.1552452977
+
+
+
+
+ 61.8948225713
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+ -90.5142857143
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -1.0
+
+
+ true
+
+
+
+
+
+
+
+
+ -36.167452007
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+ true
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+ r_gripper_l_finger_link_geom
+ 100.0
+
+ true
+ 100.0
+ r_gripper_l_finger_link
+ r_gripper_l_finger_bumper
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+ r_gripper_r_finger_link_geom
+ 100.0
+
+ true
+ r_gripper_r_finger_link
+ 100.0
+ r_gripper_r_finger_bumper
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ false
+
+ r_gripper_l_finger_tip_link_geom
+ 100.0
+
+ true
+ r_gripper_l_finger_tip_link
+ 100.0
+ r_gripper_l_finger_tip_bumper
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ false
+
+ r_gripper_r_finger_tip_link_geom
+ 100.0
+
+ true
+ r_gripper_r_finger_tip_link
+ 100.0
+ r_gripper_r_finger_tip_bumper
+
+
+
+
+
+
+
+
+
+
+ true
+ 100.0
+ r_gripper_l_finger_link
+ r_gripper_l_finger_pose_ground_truth
+ 0.0
+ base_link
+
+
+
+ true
+ 100.0
+ r_gripper_l_finger_link
+ r_gripper_l_finger_force_ground_truth
+ r_gripper_l_finger_link
+
+
+
+
+
+
+
+
+
+
+
+ true
+ 0.17126
+ 7.7562e-05
+ 1.49095e-06
+ -9.83385e-06
+ 0.000197083
+ -3.06125e-06
+ 0.000181054
+ 0.03598
+ 0.0173
+ -0.00164
+ 0.82991 -0.157 0.790675
+ 0 -0 0
+ true
+ false
+
+
+ true
+ 0.17389
+ 7.73841e-05
+ -2.09309e-06
+ -8.36228e-06
+ 0.000198474
+ 2.4611e-06
+ 0.00018107
+ 0.03576
+ -0.01736
+ -0.00095
+ 0.82991 -0.219 0.790675
+ 0 -0 0
+ true
+ false
+
+
+
+
+ r_gripper_r_parallel_link
+ r_gripper_palm_link
+ r_gripper_palm_link
+ 0 0 -1
+ 0.2
+ 0.05891 -0.031 0
+
+
+ r_gripper_l_parallel_link
+ r_gripper_palm_link
+ r_gripper_palm_link
+ 0 0 1
+ 0.2
+ 0.05891 0.031 0
+
+
+ r_gripper_r_parallel_link
+ r_gripper_r_finger_tip_link
+ r_gripper_r_finger_tip_link
+ 0 0 1
+ -0.018 -0.021 0
+
+
+ r_gripper_l_parallel_link
+ r_gripper_l_finger_tip_link
+ r_gripper_l_finger_tip_link
+ 0 0 1
+ -0.018 0.021 0
+
+
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_tip_link
+ r_gripper_r_finger_tip_link
+ 0 1 0
+
+
+
+ true
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+ r_gripper_palm_link_geom
+ 100.0
+
+ true
+ 100.0
+ r_gripper_palm_link
+ r_gripper_palm_bumper
+
+
+
+
+
+
+ true
+ 100.0
+ r_gripper_palm_link
+ r_gripper_palm_pose_ground_truth
+ 0 0 0
+ 0 0 0
+ 0.0
+ map
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+ 32.6525111499
+
+
+ true
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+ 63.1552452977
+
+
+
+
+ 61.8948225713
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+ -90.5142857143
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -1.0
+
+
+ true
+
+
+
+
+
+
+
+
+ -36.167452007
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+ true
+
+
+
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+ l_gripper_l_finger_link_geom
+ 100.0
+
+ true
+ 100.0
+ l_gripper_l_finger_link
+ l_gripper_l_finger_bumper
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+ l_gripper_r_finger_link_geom
+ 100.0
+
+ true
+ l_gripper_r_finger_link
+ 100.0
+ l_gripper_r_finger_bumper
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ false
+
+ l_gripper_l_finger_tip_link_geom
+ 100.0
+
+ true
+ l_gripper_l_finger_tip_link
+ 100.0
+ l_gripper_l_finger_tip_bumper
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ false
+
+ l_gripper_r_finger_tip_link_geom
+ 100.0
+
+ true
+ l_gripper_r_finger_tip_link
+ 100.0
+ l_gripper_r_finger_tip_bumper
+
+
+
+
+
+
+
+
+
+
+ true
+ 100.0
+ l_gripper_l_finger_link
+ l_gripper_l_finger_pose_ground_truth
+ 0.0
+ base_link
+
+
+
+ true
+ 100.0
+ l_gripper_l_finger_link
+ l_gripper_l_finger_force_ground_truth
+ l_gripper_l_finger_link
+
+
+
+
+
+
+
+
+
+
+
+ true
+ 0.17126
+ 7.7562e-05
+ 1.49095e-06
+ -9.83385e-06
+ 0.000197083
+ -3.06125e-06
+ 0.000181054
+ 0.03598
+ 0.0173
+ -0.00164
+ 0.82991 0.219 0.790675
+ 0 -0 0
+ true
+ false
+
+
+ true
+ 0.17389
+ 7.73841e-05
+ -2.09309e-06
+ -8.36228e-06
+ 0.000198474
+ 2.4611e-06
+ 0.00018107
+ 0.03576
+ -0.01736
+ -0.00095
+ 0.82991 0.157 0.790675
+ 0 -0 0
+ true
+ false
+
+
+
+
+ l_gripper_r_parallel_link
+ l_gripper_palm_link
+ l_gripper_palm_link
+ 0 0 -1
+ 0.2
+ 0.05891 -0.031 0
+
+
+ l_gripper_l_parallel_link
+ l_gripper_palm_link
+ l_gripper_palm_link
+ 0 0 1
+ 0.2
+ 0.05891 0.031 0
+
+
+ l_gripper_r_parallel_link
+ l_gripper_r_finger_tip_link
+ l_gripper_r_finger_tip_link
+ 0 0 1
+ -0.018 -0.021 0
+
+
+ l_gripper_l_parallel_link
+ l_gripper_l_finger_tip_link
+ l_gripper_l_finger_tip_link
+ 0 0 1
+ -0.018 0.021 0
+
+
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_tip_link
+ l_gripper_r_finger_tip_link
+ 0 1 0
+
+
+
+ true
+
+
+
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+ l_gripper_palm_link_geom
+ 100.0
+
+ true
+ 100.0
+ l_gripper_palm_link
+ l_gripper_palm_bumper
+
+
+
+
+
+
+ true
+ 100.0
+ l_gripper_palm_link
+ l_gripper_palm_pose_ground_truth
+ 0 0 0
+ 0 0 0
+ 0.0
+ map
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640 480
+ L8
+ 90
+ 0.1
+ 100
+ 25.0
+
+ true
+ 25.0
+ l_forearm_cam/image_raw
+ l_forearm_cam/camera_info
+ l_forearm_cam_optical_frame
+ 0
+ 320.5
+ 320.5
+ 240.5
+
+
+ 320
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+
+
+
+ true
+ PR2/Blue
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 640 480
+ L8
+ 90
+ 0.1
+ 100
+ 25.0
+
+ true
+ 25.0
+ r_forearm_cam/image_raw
+ r_forearm_cam/camera_info
+ r_forearm_cam_optical_frame
+ 0
+ 320.5
+ 320.5
+ 240.5
+
+
+ 320
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+ 0.00000001
+
+
+
+ true
+ PR2/Blue
+
+
diff --git a/srdf/test/test_parser.cpp b/srdf/test/test_parser.cpp
new file mode 100644
index 0000000..5e62f52
--- /dev/null
+++ b/srdf/test/test_parser.cpp
@@ -0,0 +1,132 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#include
+#include
+#include
+
+TEST(SRDF, Simple)
+{
+ urdf::Model u;
+ srdf::Model s;
+ EXPECT_TRUE(u.initFile("test/res/pr2_desc.urdf"));
+
+ EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.1.srdf"));
+ EXPECT_TRUE(s.getVirtualJoints().size() == 0);
+ EXPECT_TRUE(s.getGroups().size() == 0);
+ EXPECT_TRUE(s.getGroupStates().size() == 0);
+ EXPECT_TRUE(s.getDisabledCollisions().empty());
+ EXPECT_TRUE(s.getEndEffectors().size() == 0);
+
+ EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.2.srdf"));
+ EXPECT_TRUE(s.getVirtualJoints().size() == 1);
+ EXPECT_TRUE(s.getGroups().size() == 1);
+ EXPECT_TRUE(s.getGroupStates().size() == 0);
+ EXPECT_TRUE(s.getDisabledCollisions().empty());
+ EXPECT_TRUE(s.getEndEffectors().size() == 0);
+
+ EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.1.srdf"));
+ EXPECT_TRUE(s.getVirtualJoints().size() == 0);
+ EXPECT_TRUE(s.getGroups().size() == 0);
+ EXPECT_TRUE(s.getGroupStates().size() == 0);
+ EXPECT_TRUE(s.getDisabledCollisions().empty());
+ EXPECT_TRUE(s.getEndEffectors().size() == 0);
+}
+
+TEST(SRDF, Complex)
+{
+ urdf::Model u;
+ srdf::Model s;
+ EXPECT_TRUE(u.initFile("test/res/pr2_desc.urdf"));
+
+ EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.3.srdf"));
+ EXPECT_TRUE(s.getVirtualJoints().size() == 1);
+ EXPECT_TRUE(s.getGroups().size() == 7);
+ EXPECT_TRUE(s.getGroupStates().size() == 2);
+ EXPECT_TRUE(s.getDisabledCollisions().size() == 2);
+ EXPECT_TRUE(s.getEndEffectors().size() == 2);
+
+ EXPECT_EQ(s.getVirtualJoints()[0].name_, "world_joint");
+ EXPECT_EQ(s.getVirtualJoints()[0].type_, "planar");
+ for (std::size_t i = 0 ; i < s.getGroups().size() ; ++i)
+ {
+ if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
+ EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
+ if (s.getGroups()[i].name_ == "arms")
+ EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
+ if (s.getGroups()[i].name_ == "base")
+ EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
+ if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
+ {
+ EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
+ EXPECT_TRUE(s.getGroups()[i].joints_.size() == 9);
+ }
+ if (s.getGroups()[i].name_ == "whole_body")
+ {
+ EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
+ EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
+ }
+ }
+ int index = 0;
+ if (s.getGroupStates()[0].group_ != "arms")
+ index = 1;
+
+ EXPECT_EQ(s.getGroupStates()[index].group_, "arms");
+ EXPECT_EQ(s.getGroupStates()[index].name_, "tuck_arms");
+ EXPECT_EQ(s.getGroupStates()[1-index].group_, "base");
+ EXPECT_EQ(s.getGroupStates()[1-index].name_, "home");
+
+ const std::vector &v = s.getGroupStates()[index].joint_values_.find("l_shoulder_pan_joint")->second;
+ EXPECT_EQ(v.size(), 1);
+ EXPECT_EQ(v[0], 0.2);
+ const std::vector &w = s.getGroupStates()[1-index].joint_values_.find("world_joint")->second;
+ EXPECT_EQ(w.size(), 3);
+ EXPECT_EQ(w[0], 0.4);
+ EXPECT_EQ(w[1], 0);
+ EXPECT_EQ(w[2], -1);
+
+
+ index = (s.getEndEffectors()[0].name_[0] == 'r') ? 0 : 1;
+ EXPECT_EQ(s.getEndEffectors()[index].name_, "r_end_effector");
+ EXPECT_EQ(s.getEndEffectors()[index].component_group_, "r_end_effector");
+ EXPECT_EQ(s.getEndEffectors()[index].parent_link_, "r_wrist_roll_link");
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}