robot_calibration: calibration_offset_parser

Go to the documentation of this file.
00001 #include <urdf/model.h>
00002 #include <robot_calibration/calibration_offset_parser.h>
00003 #include <robot_calibration/models/chain.h>  // for rotation functions
00004 #include <gtest/gtest.h>
00005 
00006 std::string robot_description =
00007 "<?xml version='1.0' ?>"
00008 "<robot name='test'>"
00009 "  <link name='link_0'/>"
00010 "  <joint name='first_joint' type='fixed'>"
00011 "    <origin rpy='0 0 0' xyz='1 1 1'/>"
00012 "    <parent link='link_0'/>"
00013 "    <child link='link_1'/>"
00014 "  </joint>"
00015 "  <link name='link_1'/>"
00016 "  <joint name='second_joint' type='revolute'>"
00017 "    <origin rpy='0 0 0' xyz='0 0 0'/>"
00018 "    <axis xyz='0 0 1'/>"
00019 "    <limit effort='30' lower='-1.57' upper='1.57' velocity='0.524'/>"
00020 "    <parent link='link_1'/>"
00021 "    <child link='link_2'/>"
00022 "  </joint>"
00023 "  <link name='link_2'/>"
00024 "  <joint name='third_joint' type='fixed'>"
00025 "    <origin rpy='0 -1.5 0' xyz='0 0 0.0526'/>"
00026 "    <parent link='link_2'/>"
00027 "    <child link='link_3'/>"
00028 "  </joint>"
00029 "  <link name='link_3'/>"
00030 "</robot>";
00031 
00032 std::string robot_description_updated =
00033 "<?xml version=\"1.0\" ?>\n"
00034 "<robot name=\"test\">\n"
00035 "  <link name=\"link_0\" />\n"
00036 "  <joint name=\"first_joint\" type=\"fixed\">\n"
00037 "    <origin rpy=\"0 0 0\" xyz=\"1 1 1\" />\n"
00038 "    <parent link=\"link_0\" />\n"
00039 "    <child link=\"link_1\" />\n"
00040 "  </joint>\n"
00041 "  <link name=\"link_1\" />\n"
00042 "  <joint name=\"second_joint\" type=\"revolute\">\n"
00043 "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\" />\n"
00044 "    <axis xyz=\"0 0 1\" />\n"
00045 "    <limit effort=\"30\" lower=\"-1.57\" upper=\"1.57\" velocity=\"0.524\" />\n"
00046 "    <parent link=\"link_1\" />\n"
00047 "    <child link=\"link_2\" />\n"
00048 "    <calibration rising=\"0.245\" />\n"
00049 "  </joint>\n"
00050 "  <link name=\"link_2\" />\n"
00051 "  <joint name=\"third_joint\" type=\"fixed\">\n"
00052 "    <origin rpy=\"0.10000000 -1.30000000 0.30000000\" xyz=\"4.00000000 5.00000000 6.05260000\" />\n"
00053 "    <parent link=\"link_2\" />\n"
00054 "    <child link=\"link_3\" />\n"
00055 "  </joint>\n"
00056 "  <link name=\"link_3\" />\n"
00057 "</robot>";
00058 
00059 TEST(CalibrationOffsetParserTests, test_urdf_update)
00060 {
00061   robot_calibration::CalibrationOffsetParser p;
00062 
00063   p.add("second_joint");
00064   p.addFrame("third_joint", true, true, true, true, true, true);
00065 
00066   double params[7] = {0.245, 4, 5, 6, 0, 0, 0};
00067 
00068   // set angles
00069   KDL::Rotation r = KDL::Rotation::RPY(0.1, 0.2, 0.3);
00070   robot_calibration::axis_magnitude_from_rotation(r, params[4], params[5], params[6]);
00071 
00072   p.update(params);
00073 
00074   std::string s = p.updateURDF(robot_description);
00075   
00076   // google test fails if we give it all of robot_description_updated, so break this up
00077   std::vector<std::string> s_pieces;
00078   std::vector<std::string> robot_pieces;
00079 
00080   boost::split(s_pieces, s, boost::is_any_of("\n"));
00081   boost::split(robot_pieces, robot_description_updated, boost::is_any_of("\n"));
00082 
00083   for (size_t i = 0; i < robot_pieces.size(); ++i)
00084   {
00085     ASSERT_STREQ(robot_pieces[i].c_str(), s_pieces[i].c_str());
00086   }
00087 }
00088 
00089 int main(int argc, char** argv)
00090 {
00091   testing::InitGoogleTest(&argc, argv);
00092   return RUN_ALL_TESTS();
00093 }