Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

update to tinyxml2 #159

Merged
merged 1 commit into from
Nov 26, 2023
Merged

update to tinyxml2 #159

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions robot_calibration/CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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