Skip to content

Commit

Permalink
update to tinyxml2
Browse files Browse the repository at this point in the history 
* proper depends, rather than hijacking urdfdom
* less control over formatting, had to update tests
  • Loading branch information
@mikeferguson
mikeferguson committed Nov 21, 2023
1 parent 8a1dc42 commit 5f3d598
Show file tree
Hide file tree
Showing 4 changed files with 48 additions and 49 deletions.
7 changes: 4 additions & 3 deletions robot_calibration/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(tinyxml2_vendor QUIET)
find_package(TinyXML2)

find_package(orocos_kdl REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(camera_calibration_parsers REQUIRED)
Expand Down Expand Up @@ -46,6 +49,7 @@ include_directories(include
${EIGEN3_INCLUDE_DIRS}
${orocos_kdl_INCLUDE_DIRS})
link_directories(${orocos_kdl_LIBRARY_DIRS}) # this is a hack, will eventually be unneeded once orocos-kdl is fixed
link_libraries(tinyxml2::tinyxml2)

set(dependencies
camera_calibration_parsers
Expand Down Expand Up @@ -84,7 +88,6 @@ add_library(robot_calibration SHARED
target_link_libraries(robot_calibration
${Boost_LIBRARIES}
${CERES_LIBRARIES}
${tinyxml_LIBRARIES}
${orocos_kdl_LIBRARIES}
)
ament_target_dependencies(robot_calibration
Expand All @@ -101,7 +104,6 @@ add_library(robot_calibration_feature_finders SHARED
target_link_libraries(robot_calibration_feature_finders
robot_calibration
${Boost_LIBRARIES}
${tinyxml_LIBRARIES}
${orocos_kdl_LIBRARIES}
${OpenCV_LIBS}
)
Expand All @@ -114,7 +116,6 @@ target_link_libraries(calibrate
robot_calibration
${Boost_LIBRARIES}
${CERES_LIBRARIES}
${tinyxml_LIBRARIES}
${orocos_kdl_LIBRARIES}
${OpenCV_LIBS}
)
Expand Down
2 changes: 2 additions & 0 deletions robot_calibration/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@
<depend>std_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tinyxml2</depend>
<depend>tinyxml2_vendor</depend>
<depend>visualization_msgs</depend>
<depend>libceres-dev</depend>
<depend>libgflags-dev</depend>
Expand Down
42 changes: 19 additions & 23 deletions robot_calibration/src/optimization/offsets.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2018-2022 Michael Ferguson
* Copyright (C) 2018-2023 Michael Ferguson
* Copyright (C) 2013-2014 Unbounded Robotics Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
Expand All @@ -18,9 +18,10 @@
// Author: Michael Ferguson

#include <fstream>
#include <iostream>
#include <string>
#include <map>
#include <tinyxml.h>
#include <tinyxml2.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <robot_calibration/optimization/offsets.hpp>
Expand Down Expand Up @@ -221,10 +222,10 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
{
const double precision = 8;

TiXmlDocument xml_doc;
tinyxml2::XMLDocument xml_doc;
xml_doc.Parse(urdf.c_str());

TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
tinyxml2::XMLElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
// TODO: error notification? We should never get here since URDF parse
Expand All @@ -233,15 +234,16 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
}

// Update each joint
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
for (tinyxml2::XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml;
joint_xml = joint_xml->NextSiblingElement("joint"))
{
const char * name = joint_xml->Attribute("name");

// Is there a joint calibration needed?
double offset = get(std::string(name));
if (offset != 0.0)
{
TiXmlElement *calibration_xml = joint_xml->FirstChildElement("calibration");
tinyxml2::XMLElement *calibration_xml = joint_xml->FirstChildElement("calibration");
if (calibration_xml)
{
// Existing calibration, update rising attribute
Expand All @@ -251,7 +253,7 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
try
{
offset += double(boost::lexical_cast<double>(rising_position_str));
calibration_xml->SetDoubleAttribute("rising", offset);
calibration_xml->SetAttribute("rising", offset);
}
catch (boost::bad_lexical_cast &e)
{
Expand All @@ -266,10 +268,8 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
else
{
// No calibration previously, add an element + attribute
calibration_xml = new TiXmlElement("calibration");
calibration_xml->SetDoubleAttribute("rising", offset);
TiXmlNode * calibration = calibration_xml->Clone();
joint_xml->InsertEndChild(*calibration);
calibration_xml = joint_xml->InsertNewChildElement("calibration");
calibration_xml->SetAttribute("rising", offset);
}
}

Expand All @@ -283,7 +283,7 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
// String streams for output
std::stringstream xyz_ss, rpy_ss;

TiXmlElement *origin_xml = joint_xml->FirstChildElement("origin");
tinyxml2::XMLElement *origin_xml = joint_xml->FirstChildElement("origin");
if (origin_xml)
{
// Update existing origin
Expand Down Expand Up @@ -336,8 +336,8 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
}

// Update xml
origin_xml->SetAttribute("xyz", xyz_ss.str());
origin_xml->SetAttribute("rpy", rpy_ss.str());
origin_xml->SetAttribute("xyz", xyz_ss.str().c_str());
origin_xml->SetAttribute("rpy", rpy_ss.str().c_str());
}
else
{
Expand All @@ -349,7 +349,7 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
frame_offset.M.GetRPY(rpy[0], rpy[1], rpy[2]);

// No existing origin, create an element with attributes
origin_xml = new TiXmlElement("origin");
origin_xml = joint_xml->InsertNewChildElement("origin");

// Create xyz
for (int i = 0; i < 3; ++i)
Expand All @@ -358,7 +358,7 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
xyz_ss << " ";
xyz_ss << std::fixed << std::setprecision(precision) << xyz[i];
}
origin_xml->SetAttribute("xyz", xyz_ss.str());
origin_xml->SetAttribute("xyz", xyz_ss.str().c_str());

// Create rpy
for (int i = 0; i < 3; ++i)
Expand All @@ -367,18 +367,14 @@ std::string OptimizationOffsets::updateURDF(const std::string &urdf)
rpy_ss << " ";
rpy_ss << std::fixed << std::setprecision(precision) << rpy[i];
}
origin_xml->SetAttribute("rpy", rpy_ss.str());

TiXmlNode * origin = origin_xml->Clone();
joint_xml->InsertEndChild(*origin);
origin_xml->SetAttribute("rpy", rpy_ss.str().c_str());
}
}
}

// Print to a string
TiXmlPrinter printer;
printer.SetIndent(" ");
xml_doc.Accept(&printer);
tinyxml2::XMLPrinter printer;
xml_doc.Print(&printer);
std::string new_urdf = printer.CStr();

return new_urdf;
Expand Down
46 changes: 23 additions & 23 deletions robot_calibration/test/optimization_offsets_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,30 +31,30 @@ std::string robot_description =
"</robot>";

std::string robot_description_updated =
"<?xml version=\"1.0\" ?>\n"
"<?xml version='1.0' ?>\n"
"<robot name=\"test\">\n"
" <link name=\"link_0\" />\n"
" <joint name=\"first_joint\" type=\"fixed\">\n"
" <origin rpy=\"0 0 0\" xyz=\"1 1 1\" />\n"
" <parent link=\"link_0\" />\n"
" <child link=\"link_1\" />\n"
" </joint>\n"
" <link name=\"link_1\" />\n"
" <joint name=\"second_joint\" type=\"revolute\">\n"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\" />\n"
" <axis xyz=\"0 0 1\" />\n"
" <limit effort=\"30\" lower=\"-1.57\" upper=\"1.57\" velocity=\"0.524\" />\n"
" <parent link=\"link_1\" />\n"
" <child link=\"link_2\" />\n"
" <calibration rising=\"0.245\" />\n"
" </joint>\n"
" <link name=\"link_2\" />\n"
" <joint name=\"third_joint\" type=\"fixed\">\n"
" <origin rpy=\"1.57000000 -1.50000000 0.00000000\" xyz=\"0.00000000 0.00000000 0.05260000\" />\n"
" <parent link=\"link_2\" />\n"
" <child link=\"link_3\" />\n"
" </joint>\n"
" <link name=\"link_3\" />\n"
" <link name=\"link_0\"/>\n"
" <joint name=\"first_joint\" type=\"fixed\">\n"
" <origin rpy=\"0 0 0\" xyz=\"1 1 1\"/>\n"
" <parent link=\"link_0\"/>\n"
" <child link=\"link_1\"/>\n"
" </joint>\n"
" <link name=\"link_1\"/>\n"
" <joint name=\"second_joint\" type=\"revolute\">\n"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n"
" <axis xyz=\"0 0 1\"/>\n"
" <limit effort=\"30\" lower=\"-1.57\" upper=\"1.57\" velocity=\"0.524\"/>\n"
" <parent link=\"link_1\"/>\n"
" <child link=\"link_2\"/>\n"
" <calibration rising=\"0.245\"/>\n"
" </joint>\n"
" <link name=\"link_2\"/>\n"
" <joint name=\"third_joint\" type=\"fixed\">\n"
" <origin rpy=\"1.57000000 -1.50000000 0.00000000\" xyz=\"0.00000000 0.00000000 0.05260000\"/>\n"
" <parent link=\"link_2\"/>\n"
" <child link=\"link_3\"/>\n"
" </joint>\n"
" <link name=\"link_3\"/>\n"
"</robot>";

TEST(OptimizationOffsetTests, test_urdf_update)
Expand Down

0 comments on commit 5f3d598

Please sign in to comment.