meswork/smart_wasm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

规范模块目录和第三方依赖结构

Browse Source
This commit is contained in:
zhangshun
2026-06-01 16:57:39 +08:00
parent e8e485b115
commit c2ce29d874
1758 changed files with 187 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/FourBarMechanism/CrankRockingBlockMechanism_Forward.cpp
View File

@@ -1,4 +1,4 @@
#include "CrankRockingBlockMechanism_Forward.h"
#include "FourBarMechanism/CrankRockingBlockMechanism_Forward.h"
#include <cmath>
#include <algorithm>
#include <stdexcept>
@@ -477,4 +477,4 @@ string CrankRockingBlockMechanism_Forward::formatDouble(double value, int precis
char buffer[32];
snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
return string(buffer);
}
}

4
src/FourBarMechanism/CrankRockingBlockMechanism_Inverse.cpp
View File

@@ -1,4 +1,4 @@
#include "CrankRockingBlockMechanism_Inverse.h"
#include "FourBarMechanism/CrankRockingBlockMechanism_Inverse.h"
#include <cmath>
#include <algorithm>
#include <stdexcept>
@@ -258,4 +258,4 @@ string CrankRockingBlockMechanism_Inverse::getStatusText()
trajectoryPoints.size());
return string(buffer);
}
}

4
src/FourBarMechanism/CrankSliderMechanism.cpp
View File

@@ -1,5 +1,5 @@
// mechanism_simulation.cpp
#include "CrankSliderMechanism.h"
#include "FourBarMechanism/CrankSliderMechanism.h"
#include <stdexcept>
#include <algorithm>
#include <cstdio>
@@ -471,4 +471,4 @@ void deleteCrankSliderMechanism(CrankSliderMechanism *mechanism)
CrankSliderMechanismPtr createCrankSliderMechanismSmart()
{
return std::make_shared<CrankSliderMechanism>();
}
}

4
src/FourBarMechanism/FourBarMechanism.cpp
View File

@@ -1,5 +1,5 @@
// FourBarMechanism.cpp
#include "FourBarMechanism.h"
#include "FourBarMechanism/FourBarMechanism.h"
#include <stdexcept>
#include <algorithm>
#include <cstdio>
@@ -326,4 +326,4 @@ void deleteFourBarMechanism(FourBarMechanism *mechanism)
FourBarMechanismPtr createFourBarMechanismSmart()
{
return std::make_shared<FourBarMechanism>();
}
}

4
src/FourBarMechanism/SliderCrankMechanism.cpp
View File

@@ -1,5 +1,5 @@
// SliderCrankMechanism.cpp
#include "SliderCrankMechanism.h"
#include "FourBarMechanism/SliderCrankMechanism.h"
#include <stdexcept>
#include <algorithm>
#include <cstdio>
@@ -371,4 +371,4 @@ void deleteSliderCrankMechanism(SliderCrankMechanism *mechanism)
SliderCrankMechanismPtr createSliderCrankMechanismSmart()
{
return std::make_shared<SliderCrankMechanism>();
}
}

4
src/KinematicsWebAPI.cpp
View File

@@ -1,7 +1,7 @@
#include "KinematicsHelper.h"
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
#include "KinematicsWebAPI.h"
#include "CrankSliderMechanism.h"
#include "FourBarMechanism/CrankSliderMechanism.h"
#include "URDFStrings.h"
#include <iostream>

2
src/QuadrupedRobotSimulation/BaseClass.cpp
View File

@@ -1,5 +1,5 @@
// BaseClass.cpp - 修复type_error::create错误
#include "BaseClass.h"
#include "QuadrupedRobotSimulation/BaseClass.h"
// 为nlohmann/json添加std::optional支持
namespace nlohmann

8
src/QuadrupedRobotSimulation/CompleteJsonExporter.cpp
View File

@@ -1,7 +1,7 @@
// CompleteJsonExporter.cpp
#include "CompleteJsonExporter.h"
#include "BaseClass.h"
#include "KinematicsSimulation.h"
#include "QuadrupedRobotSimulation/CompleteJsonExporter.h"
#include "QuadrupedRobotSimulation/BaseClass.h"
#include "QuadrupedRobotSimulation/KinematicsSimulation.h"
#include <algorithm>
#include <cmath>
#include <functional>
@@ -711,4 +711,4 @@ std::string CompleteJsonExporter::DoubleToString(double value, int precision)
std::ostringstream oss;
oss << std::fixed << std::setprecision(precision) << value;
return oss.str();
}
}

6
src/QuadrupedRobotSimulation/KinematicsHelper.cpp
View File

@@ -1,6 +1,6 @@
// KinematicsHelper.cpp
#include "KinematicsHelper.h"
#include "RobotConfig.hpp"
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
#include "QuadrupedRobotSimulation/RobotConfig.hpp"
#include <iostream>
#include <sstream>
#include <iomanip>
@@ -317,4 +317,4 @@ RobotGaitRequest KinematicsHelper::LoadAndValidateJson(const std::string &jsonIn
std::cerr << "Error parsing JSON: " << e.what() << std::endl;
return CreateDefaultRequest();
}
}
}

8
src/QuadrupedRobotSimulation/KinematicsReverse.cpp
View File

@@ -1,7 +1,7 @@
// KinematicsReverse.cpp
#include "KinematicsReverse.h"
#include "graph_utils.hpp"
#include "RobotConfig.hpp"
#include "QuadrupedRobotSimulation/KinematicsReverse.h"
#include "QuadrupedRobotSimulation/graph_utils.hpp"
#include "QuadrupedRobotSimulation/RobotConfig.hpp"
#include <iostream>
#include <sstream>
@@ -1285,4 +1285,4 @@ void ReverseKinematicsCalculator::debugPrintFinalJson(const std::vector<C_ObjSta
std::cout << " 四元数: [" << obj.qx << ", " << obj.qy << ", "
<< obj.qz << ", " << obj.qw << "]\n";
}
}
}

6
src/QuadrupedRobotSimulation/KinematicsSimulation.cpp
View File

@@ -1,6 +1,6 @@
// KinematicsSimulation.cpp
#include "CompleteJsonExporter.h"
#include "KinematicsSimulation.h"
#include "QuadrupedRobotSimulation/CompleteJsonExporter.h"
#include "QuadrupedRobotSimulation/KinematicsSimulation.h"
#include <functional>
#include <numeric>
#include <iomanip>
@@ -1356,4 +1356,4 @@ std::vector<double> QuadrupedRobotSimulation::ShiftArray(const std::vector<doubl
result[(i + shiftFrames) % array.size()] = array[i];
}
return result;
}
}

4
src/QuadrupedRobotSimulation/RobotConfig.cpp
View File

@@ -1,4 +1,4 @@
#include "RobotConfig.hpp"
#include "QuadrupedRobotSimulation/RobotConfig.hpp"
// 统一的更新函数：手动从JSON更新现有对象
RequestParameters &RequestParameters::updateFromJson(const nlohmann::json &j, RequestParameters &req_param)
{
@@ -843,4 +843,4 @@ bool RequestParameters::saveToFile(const std::string &filename) const
{
return false;
}
}
}

4
src/QuadrupedRobotSimulation/SharedGeometry.cpp
View File

@@ -1,5 +1,5 @@
// SharedGeometry.cpp
#include "SharedGeometry.h"
#include "SharedGeometry/SharedGeometry.h"
#include <stdexcept>
#include <cmath>
#include <algorithm>
@@ -208,4 +208,4 @@ C_ObjStates PoseCalculator::CalculatePoseAndQuaternion_C_ObjStates(const std::st
c_ObjStates.qw = ToString(qw);
return c_ObjStates;
}
}

436
src/function_metadata.h
View File

@@ -1,436 +0,0 @@
#ifndef FUNCTION_METADATA_H
#define FUNCTION_METADATA_H
#include <string>
#include <vector>
#include <map>
#include <any>
#include <functional>
#include <memory>
#include <algorithm>
#include <cctype>
#include <sstream> // 添加这行
// 参数类型枚举
enum class ParamType
{
INT,
FLOAT,
DOUBLE,
BOOL,
STRING,
INT_ARRAY,
FLOAT_ARRAY,
VOID_PTR,
UNKNOWN
};
// 参数信息结构
struct ParamInfo
{
std::string name;
ParamType type;
std::any default_value;
bool is_optional;
ParamInfo(const std::string &n, ParamType t, std::any dv = {}, bool opt = false)
: name(n), type(t), default_value(dv), is_optional(opt) {}
// 获取类型名称
std::string getTypeName() const
{
switch (type)
{
case ParamType::INT:
return "int";
case ParamType::FLOAT:
return "float";
case ParamType::DOUBLE:
return "double";
case ParamType::BOOL:
return "bool";
case ParamType::STRING:
return "string";
case ParamType::INT_ARRAY:
return "int[]";
case ParamType::FLOAT_ARRAY:
return "float[]";
case ParamType::VOID_PTR:
return "void*";
default:
return "unknown";
}
}
// 类型转换
template <typename T>
T convert(const std::any &value) const
{
try
{
if constexpr (std::is_same_v<T, int>)
{
if (value.type() == typeid(int))
return std::any_cast<int>(value);
if (value.type() == typeid(double))
return static_cast<int>(std::any_cast<double>(value));
if (value.type() == typeid(float))
return static_cast<int>(std::any_cast<float>(value));
if (value.type() == typeid(std::string))
{
try
{
return std::stoi(std::any_cast<std::string>(value));
}
catch (...)
{
return 0;
}
}
return std::any_cast<T>(value);
}
else if constexpr (std::is_same_v<T, float>)
{
if (value.type() == typeid(float))
return std::any_cast<float>(value);
if (value.type() == typeid(double))
return static_cast<float>(std::any_cast<double>(value));
if (value.type() == typeid(int))
return static_cast<float>(std::any_cast<int>(value));
if (value.type() == typeid(std::string))
{
try
{
return std::stof(std::any_cast<std::string>(value));
}
catch (...)
{
return 0.0f;
}
}
return std::any_cast<T>(value);
}
else if constexpr (std::is_same_v<T, std::string>)
{
if (value.type() == typeid(std::string))
return std::any_cast<std::string>(value);
// 其他类型转换为字符串
std::stringstream ss;
if (value.type() == typeid(int))
ss << std::any_cast<int>(value);
else if (value.type() == typeid(float))
ss << std::any_cast<float>(value);
else if (value.type() == typeid(double))
ss << std::any_cast<double>(value);
else if (value.type() == typeid(bool))
ss << (std::any_cast<bool>(value) ? "true" : "false");
else
return "";
return ss.str();
}
else if constexpr (std::is_same_v<T, bool>)
{
if (value.type() == typeid(bool))
return std::any_cast<bool>(value);
if (value.type() == typeid(int))
return std::any_cast<int>(value) != 0;
if (value.type() == typeid(std::string))
{
std::string str = std::any_cast<std::string>(value);
std::string lower_str = str;
std::transform(lower_str.begin(), lower_str.end(), lower_str.begin(),
[](unsigned char c)
{ return std::tolower(c); });
return lower_str == "true" || lower_str == "1" || lower_str == "yes" || lower_str == "on";
}
return false;
}
else
{
return std::any_cast<T>(value);
}
}
catch (const std::bad_any_cast &)
{
return T();
}
}
};
// 函数信息结构
struct FunctionInfo
{
std::string name;
std::string description;
std::vector<ParamInfo> params;
std::function<std::any(const std::vector<std::any> &)> handler;
// 参数名称映射，支持多个名称
std::map<std::string, std::string> param_aliases;
FunctionInfo(const std::string &n, const std::string &desc = "")
: name(n), description(desc) {}
// 添加参数
FunctionInfo &addParam(const std::string &name, ParamType type,
std::any default_value = {}, bool optional = false)
{
params.emplace_back(name, type, default_value, optional);
return *this;
}
// 添加参数别名
FunctionInfo &addAlias(const std::string &original, const std::vector<std::string> &aliases)
{
for (const auto &alias : aliases)
{
param_aliases[alias] = original;
}
return *this;
}
// 设置处理器
template <typename Func>
FunctionInfo &setHandler(Func &&func)
{
handler = std::forward<Func>(func);
return *this;
}
// 获取规范化的参数名
std::string getCanonicalName(const std::string &input_name) const
{
// 检查别名
auto it = param_aliases.find(input_name);
if (it != param_aliases.end())
{
return it->second;
}
// 检查直接匹配（大小写不敏感）
std::string lower_input = toLower(input_name);
for (const auto &param : params)
{
std::string lower_param = toLower(param.name);
if (lower_param == lower_input)
{
return param.name;
}
}
return input_name; // 如果没有找到，返回原名称
}
// 验证参数
bool validateParams(const std::map<std::string, std::any> &input_params,
std::string &error_msg) const
{
// 检查必需参数
for (const auto &param : params)
{
if (!param.is_optional)
{
bool found = false;
for (const auto &input : input_params)
{
if (getCanonicalName(input.first) == param.name)
{
found = true;
break;
}
}
if (!found)
{
error_msg = "Missing required parameter: " + param.name;
return false;
}
}
}
return true;
}
private:
static std::string toLower(const std::string &str)
{
std::string result = str;
std::transform(result.begin(), result.end(), result.begin(),
[](unsigned char c)
{ return ::towlower(c); });
return result;
}
};
// 函数注册表
class FunctionRegistry
{
private:
std::map<std::string, std::shared_ptr<FunctionInfo>> functions_;
FunctionRegistry() = default;
public:
static FunctionRegistry &instance()
{
static FunctionRegistry instance;
return instance;
}
// 禁止复制
FunctionRegistry(const FunctionRegistry &) = delete;
FunctionRegistry &operator=(const FunctionRegistry &) = delete;
// 注册函数
void registerFunction(const std::shared_ptr<FunctionInfo> &func_info)
{
functions_[func_info->name] = func_info;
}
// 获取函数
std::shared_ptr<FunctionInfo> getFunction(const std::string &name)
{
// 直接查找
auto it = functions_.find(name);
if (it != functions_.end())
{
return it->second;
}
// 大小写不敏感查找
std::string lower_name = toLower(name);
for (const auto &[func_name, func_info] : functions_)
{
std::string lower_func = toLower(func_name);
if (lower_func == lower_name)
{
return func_info;
}
}
return nullptr;
}
// 获取所有函数
const std::map<std::string, std::shared_ptr<FunctionInfo>> &getAllFunctions() const
{
return functions_;
}
// 自动推导参数类型
ParamType deduceParamType(const std::any &value)
{
if (value.type() == typeid(int))
return ParamType::INT;
if (value.type() == typeid(float))
return ParamType::FLOAT;
if (value.type() == typeid(double))
return ParamType::DOUBLE;
if (value.type() == typeid(bool))
return ParamType::BOOL;
if (value.type() == typeid(std::string))
return ParamType::STRING;
if (value.type() == typeid(std::vector<int>))
return ParamType::INT_ARRAY;
if (value.type() == typeid(std::vector<float>))
return ParamType::FLOAT_ARRAY;
if (value.type() == typeid(void *))
return ParamType::VOID_PTR;
return ParamType::UNKNOWN;
}
// 智能参数匹配
std::map<std::string, std::any> smartMatchParams(
const std::shared_ptr<FunctionInfo> &func_info,
const std::map<std::string, std::any> &input_params)
{
std::map<std::string, std::any> matched_params;
// 1. 首先处理输入参数
for (const auto &[input_name, input_value] : input_params)
{
std::string canonical_name = func_info->getCanonicalName(input_name);
// 查找对应的参数定义
auto param_it = std::find_if(func_info->params.begin(), func_info->params.end(),
[&canonical_name](const ParamInfo &param)
{
return param.name == canonical_name;
});
if (param_it != func_info->params.end())
{
// 类型转换
std::any converted_value;
switch (param_it->type)
{
case ParamType::INT:
converted_value = param_it->convert<int>(input_value);
break;
case ParamType::FLOAT:
converted_value = param_it->convert<float>(input_value);
break;
case ParamType::STRING:
converted_value = param_it->convert<std::string>(input_value);
break;
case ParamType::BOOL:
converted_value = param_it->convert<bool>(input_value);
break;
default:
converted_value = input_value; // 保持原类型
}
matched_params[param_it->name] = converted_value;
}
}
// 2. 填充默认值
for (const auto &param : func_info->params)
{
if (matched_params.find(param.name) == matched_params.end())
{
if (!param.default_value.has_value() && !param.is_optional)
{
// 必需参数没有提供值，使用类型默认值
switch (param.type)
{
case ParamType::INT:
matched_params[param.name] = 0;
break;
case ParamType::FLOAT:
matched_params[param.name] = 0.0f;
break;
case ParamType::STRING:
matched_params[param.name] = std::string();
break;
case ParamType::BOOL:
matched_params[param.name] = false;
break;
default:
// 空值
break;
}
}
else if (param.default_value.has_value())
{
matched_params[param.name] = param.default_value;
}
}
}
return matched_params;
}
private:
static std::string toLower(const std::string &str)
{
std::string result = str;
std::transform(result.begin(), result.end(), result.begin(),
[](unsigned char c)
{ return ::towlower(c); });
return result;
}
};
#endif // FUNCTION_METADATA_H

104
src/kdl_parser/include/kdl_parser/kdl_parser.hpp
View File

@@ -1,104 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#ifndef KDL_PARSER__KDL_PARSER_HPP_
#define KDL_PARSER__KDL_PARSER_HPP_
#include <kdl/tree.hpp>
#include <string>
#include <urdf_model/model.h>
#include <tinyxml2.h>
#include <tinyxml.h> // NOLINT
#include "kdl_parser/visibility_control.hpp"
namespace kdl_parser
{
/** Constructs a KDL tree from a file, given the file name
* \param file The filename from where to read the xml
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
KDL_PARSER_PUBLIC
bool treeFromFile(const std::string& file, KDL::Tree& tree);
KDL_PARSER_PUBLIC
bool treeFromFileDocument(const std::string& file, KDL::Tree& tree);
/** Constructs a KDL tree from the parameter server, given the parameter name
* \param param the name of the parameter on the parameter server
* \param tree The resulting KDL Tree
* returns true on success, false on failure or if built without ROS
*/
KDL_PARSER_PUBLIC
bool treeFromParam(const std::string & param, KDL::Tree & tree);
/** Constructs a KDL tree from a string containing xml
* \param xml A string containing the xml description of the robot
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
KDL_PARSER_PUBLIC
bool treeFromString(const std::string & xml, KDL::Tree & tree);
/** Constructs a KDL tree from a TinyXML2 document
* \param[in] xml_doc The document containing the xml description of the robot
* \param[out] tree The resulting KDL Tree
* \return true on success, false on failure
*/
KDL_PARSER_PUBLIC
bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree);
/** Constructs a KDL tree from a TinyXML document
* \param[in] xml_doc The document containing the xml description of the robot
* \param[out] tree The resulting KDL Tree
* returns true on success, false on failure
*/
KDL_PARSER_PUBLIC
KDL_PARSER_DEPRECATED("TinyXML API is deprecated, use the TinyXML2 version instead")
bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree);
/** Constructs a KDL tree from a URDF robot model
* \param robot_model The URDF robot model
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
KDL_PARSER_PUBLIC
bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree);
} // namespace kdl_parser
#endif // KDL_PARSER__KDL_PARSER_HPP_

79
src/kdl_parser/include/kdl_parser/visibility_control.hpp
View File

@@ -1,79 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Open Source Robotics Foundation, 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 copyright holder 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.
*********************************************************************/
/* This header must be included by all kdl_parser headers which declare symbols
* which are defined in the kdl_parser library. When not building the kdl_parser
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the kdl_parser
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef KDL_PARSER__VISIBILITY_CONTROL_HPP_
#define KDL_PARSER__VISIBILITY_CONTROL_HPP_
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define KDL_PARSER_EXPORT __attribute__ ((dllexport))
#define KDL_PARSER_IMPORT __attribute__ ((dllimport))
#define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg)))
#else
#define KDL_PARSER_EXPORT __declspec(dllexport)
#define KDL_PARSER_IMPORT __declspec(dllimport)
#define KDL_PARSER_DEPRECATED(msg) __declspec(deprecated(msg))
#endif
#ifdef KDL_PARSER_BUILDING_DLL
#define KDL_PARSER_PUBLIC KDL_PARSER_EXPORT
#else
#define KDL_PARSER_PUBLIC KDL_PARSER_IMPORT
#endif
#define KDL_PARSER_PUBLIC_TYPE KDL_PARSER_PUBLIC
#define KDL_PARSER_LOCAL
#else
#define KDL_PARSER_EXPORT __attribute__ ((visibility("default")))
#define KDL_PARSER_IMPORT
#if __GNUC__ >= 4
#define KDL_PARSER_PUBLIC __attribute__ ((visibility("default")))
#define KDL_PARSER_LOCAL __attribute__ ((visibility("hidden")))
#else
#define KDL_PARSER_PUBLIC
#define KDL_PARSER_LOCAL
#endif
#define KDL_PARSER_PUBLIC_TYPE
#define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg)))
#endif
#endif // KDL_PARSER__VISIBILITY_CONTROL_HPP_

305
src/kdl_parser/include/tinystr.h
View File

@@ -1,305 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#ifndef TIXML_USE_STL
#ifndef TIXML_STRING_INCLUDED
#define TIXML_STRING_INCLUDED
#include <assert.h>
#include <string.h>
/* The support for explicit isn't that universal, and it isn't really
required - it is used to check that the TiXmlString class isn't incorrectly
used. Be nice to old compilers and macro it here:
*/
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
// Microsoft visual studio, version 6 and higher.
#define TIXML_EXPLICIT explicit
#elif defined(__GNUC__) && (__GNUC__ >= 3 )
// GCC version 3 and higher.s
#define TIXML_EXPLICIT explicit
#else
#define TIXML_EXPLICIT
#endif
/*
TiXmlString is an emulation of a subset of the std::string template.
Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
Only the member functions relevant to the TinyXML project have been implemented.
The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
a string and there's no more room, we allocate a buffer twice as big as we need.
*/
class TiXmlString
{
public :
// The size type used
typedef size_t size_type;
// Error value for find primitive
static const size_type npos; // = -1;
// TiXmlString empty constructor
TiXmlString () : rep_(&nullrep_)
{
}
// TiXmlString copy constructor
TiXmlString ( const TiXmlString & copy) : rep_(0)
{
init(copy.length());
memcpy(start(), copy.data(), length());
}
// TiXmlString constructor, based on a string
TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
{
init( static_cast<size_type>( strlen(copy) ));
memcpy(start(), copy, length());
}
// TiXmlString constructor, based on a string
TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
{
init(len);
memcpy(start(), str, len);
}
// TiXmlString destructor
~TiXmlString ()
{
quit();
}
TiXmlString& operator = (const char * copy)
{
return assign( copy, (size_type)strlen(copy));
}
TiXmlString& operator = (const TiXmlString & copy)
{
return assign(copy.start(), copy.length());
}
// += operator. Maps to append
TiXmlString& operator += (const char * suffix)
{
return append(suffix, static_cast<size_type>( strlen(suffix) ));
}
// += operator. Maps to append
TiXmlString& operator += (char single)
{
return append(&single, 1);
}
// += operator. Maps to append
TiXmlString& operator += (const TiXmlString & suffix)
{
return append(suffix.data(), suffix.length());
}
// Convert a TiXmlString into a null-terminated char *
const char * c_str () const { return rep_->str; }
// Convert a TiXmlString into a char * (need not be null terminated).
const char * data () const { return rep_->str; }
// Return the length of a TiXmlString
size_type length () const { return rep_->size; }
// Alias for length()
size_type size () const { return rep_->size; }
// Checks if a TiXmlString is empty
bool empty () const { return rep_->size == 0; }
// Return capacity of string
size_type capacity () const { return rep_->capacity; }
// single char extraction
const char& at (size_type index) const
{
assert( index < length() );
return rep_->str[ index ];
}
// [] operator
char& operator [] (size_type index) const
{
assert( index < length() );
return rep_->str[ index ];
}
// find a char in a string. Return TiXmlString::npos if not found
size_type find (char lookup) const
{
return find(lookup, 0);
}
// find a char in a string from an offset. Return TiXmlString::npos if not found
size_type find (char tofind, size_type offset) const
{
if (offset >= length()) return npos;
for (const char* p = c_str() + offset; *p != '\0'; ++p)
{
if (*p == tofind) return static_cast< size_type >( p - c_str() );
}
return npos;
}
void clear ()
{
//Lee:
//The original was just too strange, though correct:
// TiXmlString().swap(*this);
//Instead use the quit & re-init:
quit();
init(0,0);
}
/* Function to reserve a big amount of data when we know we'll need it. Be aware that this
function DOES NOT clear the content of the TiXmlString if any exists.
*/
void reserve (size_type cap);
TiXmlString& assign (const char* str, size_type len);
TiXmlString& append (const char* str, size_type len);
void swap (TiXmlString& other)
{
Rep* r = rep_;
rep_ = other.rep_;
other.rep_ = r;
}
private:
void init(size_type sz) { init(sz, sz); }
void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
char* start() const { return rep_->str; }
char* finish() const { return rep_->str + rep_->size; }
struct Rep
{
size_type size, capacity;
char str[1];
};
void init(size_type sz, size_type cap)
{
if (cap)
{
// Lee: the original form:
// rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
// doesn't work in some cases of new being overloaded. Switching
// to the normal allocation, although use an 'int' for systems
// that are overly picky about structure alignment.
const size_type bytesNeeded = sizeof(Rep) + cap;
const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int );
rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
rep_->str[ rep_->size = sz ] = '\0';
rep_->capacity = cap;
}
else
{
rep_ = &nullrep_;
}
}
void quit()
{
if (rep_ != &nullrep_)
{
// The rep_ is really an array of ints. (see the allocator, above).
// Cast it back before delete, so the compiler won't incorrectly call destructors.
delete [] ( reinterpret_cast<int*>( rep_ ) );
}
}
Rep * rep_;
static Rep nullrep_;
} ;
inline bool operator == (const TiXmlString & a, const TiXmlString & b)
{
return ( a.length() == b.length() ) // optimization on some platforms
&& ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare
}
inline bool operator < (const TiXmlString & a, const TiXmlString & b)
{
return strcmp(a.c_str(), b.c_str()) < 0;
}
inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; }
inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
TiXmlString operator + (const TiXmlString & a, const char* b);
TiXmlString operator + (const char* a, const TiXmlString & b);
/*
TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
Only the operators that we need for TinyXML have been developped.
*/
class TiXmlOutStream : public TiXmlString
{
public :
// TiXmlOutStream << operator.
TiXmlOutStream & operator << (const TiXmlString & in)
{
*this += in;
return *this;
}
// TiXmlOutStream << operator.
TiXmlOutStream & operator << (const char * in)
{
*this += in;
return *this;
}
} ;
#endif // TIXML_STRING_INCLUDED
#endif // TIXML_USE_STL

1805
src/kdl_parser/include/tinyxml.h
View File

File diff suppressed because it is too large Load Diff

2380
src/kdl_parser/include/tinyxml2.h
View File

File diff suppressed because it is too large Load Diff

53
src/kdl_parser/include/urdf_exception/exception.h
View File

@@ -1,53 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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.
*********************************************************************/
// URDF exceptions
#ifndef URDF_INTERFACE_EXCEPTION_H_
#define URDF_INTERFACE_EXCEPTION_H_
#include <string>
#include <stdexcept>
namespace urdf
{
class ParseError: public std::runtime_error
{
public:
ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {};
};
}
#endif

105
src/kdl_parser/include/urdf_model/color.h
View File

@@ -1,105 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Josh Faust */
#ifndef URDF_INTERFACE_COLOR_H
#define URDF_INTERFACE_COLOR_H
#include <stdexcept>
#include <string>
#include <vector>
#include <math.h>
#include <urdf_model/utils.h>
#include <urdf_exception/exception.h>
namespace urdf
{
class Color
{
public:
Color() {this->clear();};
float r;
float g;
float b;
float a;
void clear()
{
r = g = b = 0.0f;
a = 1.0f;
}
bool init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<float> rgba;
urdf::split_string( pieces, vector_str, " ");
for (unsigned int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
try
{
double piece = strToDouble(pieces[i].c_str());
if ((piece < 0) || (piece > 1))
throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]");
rgba.push_back(static_cast<float>(piece));
}
catch (std::runtime_error &/*e*/) {
throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)");
}
}
}
if (rgba.size() != 4)
{
return false;
}
this->r = rgba[0];
this->g = rgba[1];
this->b = rgba[2];
this->a = rgba[3];
return true;
};
};
}
#endif

230
src/kdl_parser/include/urdf_model/joint.h
View File

@@ -1,230 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#ifndef URDF_INTERFACE_JOINT_H
#define URDF_INTERFACE_JOINT_H
#include <string>
#include <vector>
#include "urdf_model/pose.h"
#include "urdf_model/types.h"
namespace urdf{
class Link;
class JointDynamics
{
public:
JointDynamics() { this->clear(); };
double damping;
double friction;
void clear()
{
damping = 0;
friction = 0;
};
};
class JointLimits
{
public:
JointLimits() { this->clear(); };
double lower;
double upper;
double effort;
double velocity;
void clear()
{
lower = 0;
upper = 0;
effort = 0;
velocity = 0;
};
};
/// \brief Parameters for Joint Safety Controllers
class JointSafety
{
public:
/// clear variables on construction
JointSafety() { this->clear(); };
///
/// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage.
///
/// Basic safety controller operation is as follows
///
/// current safety controllers will take effect on joints outside the position range below:
///
/// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position,
/// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position]
///
/// if (joint_position is outside of the position range above)
/// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit)
/// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit)
/// else
/// velocity_limit_min = -JointLimits::velocity
/// velocity_limit_max = JointLimits::velocity
///
/// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity,
/// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity]
///
/// if (joint_velocity is outside of the velocity range above)
/// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min)
/// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max)
/// else
/// effort_limit_min = -JointLimits::effort
/// effort_limit_max = JointLimits::effort
///
/// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max]
///
/// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits
///
double soft_upper_limit;
double soft_lower_limit;
double k_position;
double k_velocity;
void clear()
{
soft_upper_limit = 0;
soft_lower_limit = 0;
k_position = 0;
k_velocity = 0;
};
};
class JointCalibration
{
public:
JointCalibration() { this->clear(); };
double reference_position;
DoubleSharedPtr rising, falling;
void clear()
{
reference_position = 0;
};
};
class JointMimic
{
public:
JointMimic() { this->clear(); };
double offset;
double multiplier;
std::string joint_name;
void clear()
{
offset = 0.0;
multiplier = 0.0;
joint_name.clear();
};
};
class Joint
{
public:
Joint() { this->clear(); };
std::string name;
enum
{
UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
} type;
/// \brief type_ meaning of axis_
/// ------------------------------------------------------
/// UNKNOWN unknown type
/// REVOLUTE rotation axis
/// PRISMATIC translation axis
/// FLOATING N/A
/// PLANAR plane normal axis
/// FIXED N/A
Vector3 axis;
/// child Link element
/// child link frame is the same as the Joint frame
std::string child_link_name;
/// parent Link element
/// origin specifies the transform from Parent Link to Joint Frame
std::string parent_link_name;
/// transform from Parent Link frame to Joint frame
Pose parent_to_joint_origin_transform;
/// Joint Dynamics
JointDynamicsSharedPtr dynamics;
/// Joint Limits
JointLimitsSharedPtr limits;
/// Unsupported Hidden Feature
JointSafetySharedPtr safety;
/// Unsupported Hidden Feature
JointCalibrationSharedPtr calibration;
/// Option to Mimic another Joint
JointMimicSharedPtr mimic;
void clear()
{
this->axis.clear();
this->child_link_name.clear();
this->parent_link_name.clear();
this->parent_to_joint_origin_transform.clear();
this->dynamics.reset();
this->limits.reset();
this->safety.reset();
this->calibration.reset();
this->mimic.reset();
this->type = UNKNOWN;
};
};
}
#endif

247
src/kdl_parser/include/urdf_model/link.h
View File

@@ -1,247 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#ifndef URDF_INTERFACE_LINK_H
#define URDF_INTERFACE_LINK_H
#include <string>
#include <vector>
#include <map>
#include "joint.h"
#include "color.h"
#include "types.h"
namespace urdf{
class Geometry
{
public:
enum {SPHERE, BOX, CYLINDER, MESH} type;
virtual ~Geometry(void)
{
}
};
class Sphere : public Geometry
{
public:
Sphere() { this->clear(); type = SPHERE; };
double radius;
void clear()
{
radius = 0;
};
};
class Box : public Geometry
{
public:
Box() { this->clear(); type = BOX; };
Vector3 dim;
void clear()
{
this->dim.clear();
};
};
class Cylinder : public Geometry
{
public:
Cylinder() { this->clear(); type = CYLINDER; };
double length;
double radius;
void clear()
{
length = 0;
radius = 0;
};
};
class Mesh : public Geometry
{
public:
Mesh() { this->clear(); type = MESH; };
std::string filename;
Vector3 scale;
void clear()
{
filename.clear();
// default scale
scale.x = 1;
scale.y = 1;
scale.z = 1;
};
};
class Material
{
public:
Material() { this->clear(); };
std::string name;
std::string texture_filename;
Color color;
void clear()
{
color.clear();
texture_filename.clear();
name.clear();
};
};
class Inertial
{
public:
Inertial() { this->clear(); };
Pose origin;
double mass;
double ixx,ixy,ixz,iyy,iyz,izz;
void clear()
{
origin.clear();
mass = 0;
ixx = ixy = ixz = iyy = iyz = izz = 0;
};
};
class Visual
{
public:
Visual() { this->clear(); };
Pose origin;
GeometrySharedPtr geometry;
std::string material_name;
MaterialSharedPtr material;
void clear()
{
origin.clear();
material_name.clear();
material.reset();
geometry.reset();
name.clear();
};
std::string name;
};
class Collision
{
public:
Collision() { this->clear(); };
Pose origin;
GeometrySharedPtr geometry;
void clear()
{
origin.clear();
geometry.reset();
name.clear();
};
std::string name;
};
class Link
{
public:
Link() { this->clear(); };
std::string name;
/// inertial element
InertialSharedPtr inertial;
/// visual element
VisualSharedPtr visual;
/// collision element
CollisionSharedPtr collision;
/// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array)
std::vector<CollisionSharedPtr> collision_array;
/// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array)
std::vector<VisualSharedPtr> visual_array;
/// Parent Joint element
/// explicitly stating "parent" because we want directional-ness for tree structure
/// every link can have one parent
JointSharedPtr parent_joint;
std::vector<JointSharedPtr> child_joints;
std::vector<LinkSharedPtr> child_links;
LinkSharedPtr getParent() const
{return parent_link_.lock();};
void setParent(const LinkSharedPtr &parent)
{ parent_link_ = parent; }
void clear()
{
this->name.clear();
this->inertial.reset();
this->visual.reset();
this->collision.reset();
this->parent_joint.reset();
this->child_joints.clear();
this->child_links.clear();
this->collision_array.clear();
this->visual_array.clear();
};
private:
LinkWeakPtr parent_link_;
};
}
#endif

206
src/kdl_parser/include/urdf_model/model.h
View File

@@ -1,206 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#ifndef URDF_INTERFACE_MODEL_H
#define URDF_INTERFACE_MODEL_H
#include <string>
#include <map>
#include <urdf_model/link.h>
#include <urdf_model/types.h>
#include <urdf_exception/exception.h>
namespace urdf {
class ModelInterface
{
public:
LinkConstSharedPtr getRoot(void) const{return this->root_link_;};
LinkConstSharedPtr getLink(const std::string& name) const
{
LinkConstSharedPtr ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
return ptr;
};
JointConstSharedPtr getJoint(const std::string& name) const
{
JointConstSharedPtr ptr;
if (this->joints_.find(name) == this->joints_.end())
ptr.reset();
else
ptr = this->joints_.find(name)->second;
return ptr;
};
const std::string& getName() const {return name_;};
void getLinks(std::vector<LinkSharedPtr >& links) const
{
for (std::map<std::string,LinkSharedPtr>::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
{
links.push_back(link->second);
}
};
void clear()
{
name_.clear();
this->links_.clear();
this->joints_.clear();
this->materials_.clear();
this->root_link_.reset();
};
/// non-const getLink()
void getLink(const std::string& name, LinkSharedPtr &link) const
{
LinkSharedPtr ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
link = ptr;
};
/// non-const getMaterial()
MaterialSharedPtr getMaterial(const std::string& name) const
{
MaterialSharedPtr ptr;
if (this->materials_.find(name) == this->materials_.end())
ptr.reset();
else
ptr = this->materials_.find(name)->second;
return ptr;
};
void initTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string, JointSharedPtr>::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
if (parent_link_name.empty() || child_link_name.empty())
{
throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
}
else
{
// find child and parent links
LinkSharedPtr child_link, parent_link;
this->getLink(child_link_name, child_link);
if (!child_link)
{
throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
}
this->getLink(parent_link_name, parent_link);
if (!parent_link)
{
throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
}
//set parent link for child link
child_link->setParent(parent_link);
//set parent joint for child link
child_link->parent_joint = joint->second;
//set child joint for parent link
parent_link->child_joints.push_back(joint->second);
//set child link for parent link
parent_link->child_links.push_back(child_link);
// fill in child/parent string map
parent_link_tree[child_link->name] = parent_link_name;
}
}
}
void initRoot(const std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
// find the links that have no parent in the tree
for (std::map<std::string, LinkSharedPtr>::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first);
if (parent == parent_link_tree.end())
{
// store root link
if (!this->root_link_)
{
getLink(l->first, this->root_link_);
}
// we already found a root link
else
{
throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
}
}
}
if (!this->root_link_)
{
throw ParseError("No root link found. The robot xml is not a valid tree.");
}
}
/// \brief complete list of Links
std::map<std::string, LinkSharedPtr> links_;
/// \brief complete list of Joints
std::map<std::string, JointSharedPtr> joints_;
/// \brief complete list of Materials
std::map<std::string, MaterialSharedPtr> materials_;
/// \brief The name of the robot model
std::string name_;
/// \brief The root is always a link (the parent of the tree describing the robot)
LinkSharedPtr root_link_;
};
}
#endif

260
src/kdl_parser/include/urdf_model/pose.h
View File

@@ -1,260 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#ifndef URDF_INTERFACE_POSE_H
#define URDF_INTERFACE_POSE_H
#include <cmath>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <urdf_exception/exception.h>
#include <urdf_model/utils.h>
namespace urdf{
class Vector3
{
public:
Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
Vector3() {this->clear();};
double x;
double y;
double z;
void clear() {this->x=this->y=this->z=0.0;};
void init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<double> xyz;
urdf::split_string( pieces, vector_str, " ");
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
try {
xyz.push_back(strToDouble(pieces[i].c_str()));
} catch(std::runtime_error &) {
throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
}
}
}
if (xyz.size() != 3)
throw ParseError("Parser found " + std::to_string(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]");
this->x = xyz[0];
this->y = xyz[1];
this->z = xyz[2];
}
Vector3 operator+(Vector3 vec)
{
return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
};
};
class Rotation
{
public:
Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
Rotation() {this->clear();};
void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
{
quat_x = this->x;
quat_y = this->y;
quat_z = this->z;
quat_w = this->w;
};
void getRPY(double &roll,double &pitch,double &yaw) const
{
double sqw;
double sqx;
double sqy;
double sqz;
sqx = this->x * this->x;
sqy = this->y * this->y;
sqz = this->z * this->z;
sqw = this->w * this->w;
// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
double sarg = -2 * (this->x*this->z - this->w*this->y);
const double pi_2 = 1.57079632679489661923;
if (sarg <= -0.99999) {
pitch = -pi_2;
roll = 0;
yaw = 2 * atan2(this->x, -this->y);
} else if (sarg >= 0.99999) {
pitch = pi_2;
roll = 0;
yaw = 2 * atan2(-this->x, this->y);
} else {
pitch = asin(sarg);
roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
}
};
void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
{
this->x = quat_x;
this->y = quat_y;
this->z = quat_z;
this->w = quat_w;
this->normalize();
};
void setFromRPY(double roll, double pitch, double yaw)
{
double phi, the, psi;
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
this->normalize();
};
double x,y,z,w;
void init(const std::string &rotation_str)
{
this->clear();
Vector3 rpy;
rpy.init(rotation_str);
setFromRPY(rpy.x, rpy.y, rpy.z);
}
void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
void normalize()
{
double s = sqrt(this->x * this->x +
this->y * this->y +
this->z * this->z +
this->w * this->w);
if (s == 0.0)
{
this->x = 0.0;
this->y = 0.0;
this->z = 0.0;
this->w = 1.0;
}
else
{
this->x /= s;
this->y /= s;
this->z /= s;
this->w /= s;
}
};
// Multiplication operator (copied from gazebo)
Rotation operator*( const Rotation &qt ) const
{
Rotation c;
c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
return c;
};
/// Rotate a vector using the quaternion
Vector3 operator*(Vector3 vec) const
{
Rotation tmp;
Vector3 result;
tmp.w = 0.0;
tmp.x = vec.x;
tmp.y = vec.y;
tmp.z = vec.z;
tmp = (*this) * (tmp * this->GetInverse());
result.x = tmp.x;
result.y = tmp.y;
result.z = tmp.z;
return result;
};
// Get the inverse of this quaternion
Rotation GetInverse() const
{
Rotation q;
double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
if (norm > 0.0)
{
q.w = this->w / norm;
q.x = -this->x / norm;
q.y = -this->y / norm;
q.z = -this->z / norm;
}
return q;
};
};
class Pose
{
public:
Pose() { this->clear(); };
Vector3 position;
Rotation rotation;
void clear()
{
this->position.clear();
this->rotation.clear();
};
};
}
#endif

68
src/kdl_parser/include/urdf_model/twist.h
View File

@@ -1,68 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
#ifndef URDF_TWIST_H
#define URDF_TWIST_H
#include <string>
#include <sstream>
#include <vector>
#include <math.h>
#include <urdf_model/pose.h>
namespace urdf{
class Twist
{
public:
Twist() { this->clear(); };
Vector3 linear;
// Angular velocity represented by Euler angles
Vector3 angular;
void clear()
{
this->linear.clear();
this->angular.clear();
};
};
}
#endif

91
src/kdl_parser/include/urdf_model/types.h
View File

@@ -1,91 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Steve Peters */
#ifndef URDF_MODEL_TYPES_H
#define URDF_MODEL_TYPES_H
#include <memory>
#define URDF_TYPEDEF_CLASS_POINTER(Class) \
class Class; \
typedef std::shared_ptr<Class> Class##SharedPtr; \
typedef std::shared_ptr<const Class> Class##ConstSharedPtr; \
typedef std::weak_ptr<Class> Class##WeakPtr
namespace urdf{
// shared pointer used in joint.h
typedef std::shared_ptr<double> DoubleSharedPtr;
URDF_TYPEDEF_CLASS_POINTER(Box);
URDF_TYPEDEF_CLASS_POINTER(Collision);
URDF_TYPEDEF_CLASS_POINTER(Cylinder);
URDF_TYPEDEF_CLASS_POINTER(Geometry);
URDF_TYPEDEF_CLASS_POINTER(Inertial);
URDF_TYPEDEF_CLASS_POINTER(Joint);
URDF_TYPEDEF_CLASS_POINTER(JointCalibration);
URDF_TYPEDEF_CLASS_POINTER(JointDynamics);
URDF_TYPEDEF_CLASS_POINTER(JointLimits);
URDF_TYPEDEF_CLASS_POINTER(JointMimic);
URDF_TYPEDEF_CLASS_POINTER(JointSafety);
URDF_TYPEDEF_CLASS_POINTER(Link);
URDF_TYPEDEF_CLASS_POINTER(Material);
URDF_TYPEDEF_CLASS_POINTER(Mesh);
URDF_TYPEDEF_CLASS_POINTER(Sphere);
URDF_TYPEDEF_CLASS_POINTER(Visual);
// create *_pointer_cast functions in urdf namespace
template<class T, class U>
std::shared_ptr<T> const_pointer_cast(std::shared_ptr<U> const & r)
{
return std::const_pointer_cast<T>(r);
}
template<class T, class U>
std::shared_ptr<T> dynamic_pointer_cast(std::shared_ptr<U> const & r)
{
return std::dynamic_pointer_cast<T>(r);
}
template<class T, class U>
std::shared_ptr<T> static_pointer_cast(std::shared_ptr<U> const & r)
{
return std::static_pointer_cast<T>(r);
}
}
#endif

92
src/kdl_parser/include/urdf_model/utils.h
View File

@@ -1,92 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2016, Open Source Robotics Foundation (OSRF)
* 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 OSRF 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: Steve Peters */
#ifndef URDF_INTERFACE_UTILS_H
#define URDF_INTERFACE_UTILS_H
#include <locale>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
namespace urdf {
// Replacement for boost::split( ... , ... , boost::is_any_of(" "))
inline
void split_string(std::vector<std::string> &result,
const std::string &input,
const std::string &isAnyOf)
{
std::string::size_type start = 0;
std::string::size_type end = input.find_first_of(isAnyOf, start);
while (end != std::string::npos)
{
result.push_back(input.substr(start, end-start));
start = end + 1;
end = input.find_first_of(isAnyOf, start);
}
if (start < input.length())
{
result.push_back(input.substr(start));
}
}
// This is a locale-safe version of string-to-double, which is suprisingly
// difficult to do correctly. This function ensures that the C locale is used
// for parsing, as that matches up with what the XSD for double specifies.
// On success, the double is returned; on failure, a std::runtime_error is
// thrown.
static inline double strToDouble(const char *in)
{
std::stringstream ss;
ss.imbue(std::locale::classic());
ss << in;
double out;
ss >> out;
if (ss.fail() || !ss.eof()) {
throw std::runtime_error("Failed converting string to double");
}
return out;
}
}
#endif

151
src/kdl_parser/include/urdf_model_state/model_state.h
View File

@@ -1,151 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
#ifndef URDF_MODEL_STATE_H
#define URDF_MODEL_STATE_H
#include <string>
#include <vector>
#include <map>
#include "urdf_model/pose.h"
#include <urdf_model/twist.h>
#include "urdf_model_state/types.h"
namespace urdf{
//round is not defined in C++98
//So in Visual Studio <= 2012 is necessary to define it
#ifdef _MSC_VER
#if (_MSC_VER <= 1700)
double round(double value)
{
return (value >= 0.0f)?(floor(value + 0.5f)):(ceil(value - 0.5f));
}
#endif
#endif
class Time
{
public:
Time() { this->clear(); };
void set(double _seconds)
{
this->sec = (int32_t)(floor(_seconds));
this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9));
this->Correct();
};
operator double ()
{
return (static_cast<double>(this->sec) +
static_cast<double>(this->nsec)*1e-9);
};
int32_t sec;
int32_t nsec;
void clear()
{
this->sec = 0;
this->nsec = 0;
};
private:
void Correct()
{
// Make any corrections
if (this->nsec >= 1e9)
{
this->sec++;
this->nsec = (int32_t)(this->nsec - 1e9);
}
else if (this->nsec < 0)
{
this->sec--;
this->nsec = (int32_t)(this->nsec + 1e9);
}
};
};
class JointState
{
public:
JointState() { this->clear(); };
/// joint name
std::string joint;
std::vector<double> position;
std::vector<double> velocity;
std::vector<double> effort;
void clear()
{
this->joint.clear();
this->position.clear();
this->velocity.clear();
this->effort.clear();
}
};
class ModelState
{
public:
ModelState() { this->clear(); };
/// state name must be unique
std::string name;
Time time_stamp;
void clear()
{
this->name.clear();
this->time_stamp.set(0);
this->joint_states.clear();
};
std::vector<JointStateSharedPtr> joint_states;
};
}
#endif

42
src/kdl_parser/include/urdf_model_state/twist.h
View File

@@ -1,42 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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.
*********************************************************************/
#ifndef URDF_MODEL_STATE_TWIST_
#define URDF_MODEL_STATE_TWIST_
#warning "Please Use #include <urdf_model/twist.h>"
#include <urdf_model/twist.h>
#endif

52
src/kdl_parser/include/urdf_model_state/types.h
View File

@@ -1,52 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Steve Peters */
#ifndef URDF_MODEL_STATE_TYPES_H
#define URDF_MODEL_STATE_TYPES_H
#include <memory>
namespace urdf{
class JointState;
// typedef shared pointers
typedef std::shared_ptr<JointState> JointStateSharedPtr;
}
#endif

84
src/kdl_parser/include/urdf_parser/exportdecl.h
View File

@@ -1,84 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Thomas Moulard */
#ifndef URDFDOM_EXPORTDECL_H
# define URDFDOM_EXPORTDECL_H
// Handle portable symbol export.
// Defining manually which symbol should be exported is required
// under Windows whether MinGW or MSVC is used.
//
// The headers then have to be able to work in two different modes:
// - dllexport when one is building the library,
// - dllimport for clients using the library.
//
// On Linux, set the visibility accordingly. If C++ symbol visibility
// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
# if defined _WIN32 || defined __CYGWIN__
// On Microsoft Windows, use dllimport and dllexport to tag symbols.
# define URDFDOM_DLLIMPORT __declspec(dllimport)
# define URDFDOM_DLLEXPORT __declspec(dllexport)
# define URDFDOM_DLLLOCAL
# else
// On Linux, for GCC >= 4, tag symbols using GCC extension.
# if __GNUC__ >= 4
# define URDFDOM_DLLIMPORT __attribute__ ((visibility("default")))
# define URDFDOM_DLLEXPORT __attribute__ ((visibility("default")))
# define URDFDOM_DLLLOCAL __attribute__ ((visibility("hidden")))
# else
// Otherwise (GCC < 4 or another compiler is used), export everything.
# define URDFDOM_DLLIMPORT
# define URDFDOM_DLLEXPORT
# define URDFDOM_DLLLOCAL
# endif // __GNUC__ >= 4
# endif // defined _WIN32 || defined __CYGWIN__
# ifdef URDFDOM_STATIC
// If one is using the library statically, get rid of
// extra information.
# define URDFDOM_DLLAPI
# define URDFDOM_LOCAL
# else
// Depending on whether one is building or using the
// library define DLLAPI to import or export.
# ifdef URDFDOM_EXPORTS
# define URDFDOM_DLLAPI URDFDOM_DLLEXPORT
# else
# define URDFDOM_DLLAPI URDFDOM_DLLIMPORT
# endif // URDFDOM_EXPORTS
# define URDFDOM_LOCAL URDFDOM_DLLLOCAL
# endif // URDFDOM_STATIC
#endif //! URDFDOM_EXPORTDECL_H

150
src/kdl_parser/include/urdf_parser/urdf_parser.h
View File

@@ -1,150 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#ifndef URDF_PARSER_URDF_PARSER_H
#define URDF_PARSER_URDF_PARSER_H
#include <stdexcept>
#include <string>
#include <vector>
#include <tinyxml.h>
#include <urdf_model/model.h>
#include <urdf_model/color.h>
#include <urdf_world/types.h>
#include <urdf_model/utils.h>
#include "exportdecl.h"
namespace urdf_export_helpers {
URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL);
URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec);
URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot);
URDFDOM_DLLAPI std::string values2str(urdf::Color c);
URDFDOM_DLLAPI std::string values2str(double d);
// This lives here (rather than in model.cpp) so we can run tests on it.
class URDFVersion final
{
public:
explicit URDFVersion(const char *attr)
{
// If the passed in attribute is NULL, it means it wasn't specified in the
// XML, so we just assume version 1.0.
if (attr == nullptr)
{
major_ = 1;
minor_ = 0;
return;
}
// We only accept version strings of the type <major>.<minor>
std::vector<std::string> split;
urdf::split_string(split, std::string(attr), ".");
if (split.size() == 2)
{
major_ = strToUnsigned(split[0].c_str());
minor_ = strToUnsigned(split[1].c_str());
}
else
{
throw std::runtime_error("The version attribute should be in the form 'x.y'");
}
}
bool equal(uint32_t maj, uint32_t min)
{
return this->major_ == maj && this->minor_ == min;
}
uint32_t getMajor() const
{
return major_;
}
uint32_t getMinor() const
{
return minor_;
}
private:
uint32_t strToUnsigned(const char *str)
{
if (str[0] == '\0')
{
// This would get caught below, but we can make a nicer error message
throw std::runtime_error("One of the fields of the version attribute is blank");
}
char *end = const_cast<char *>(str);
long value = strtol(str, &end, 10);
if (end == str)
{
// If the pointer didn't move at all, then we couldn't convert any of
// the string to an integer.
throw std::runtime_error("Version attribute is not an integer");
}
if (*end != '\0')
{
// Here, we didn't go all the way to the end of the string, which
// means there was junk at the end
throw std::runtime_error("Extra characters after the version number");
}
if (value < 0)
{
throw std::runtime_error("Version number must be positive");
}
return value;
}
uint32_t major_;
uint32_t minor_;
};
}
namespace urdf{
URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string);
URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string& path);
URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFileDocument(const std::string &path);
URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model);
URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model);
URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*);
}
#endif

176
src/kdl_parser/include/urdf_sensor/sensor.h
View File

@@ -1,176 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
/* example
<sensor name="my_camera_sensor" update_rate="20">
<origin xyz="0 0 0" rpy="0 0 0"/>
<camera>
<horizontal_hov>1.5708</horizontal_hov>
<image width="640" height="480" format="R8G8B8"/>
<clip near="0.01" far="50.0"/>
</camera>
</sensor>
<sensor name="my_ray_sensor" update_rate="20">
<origin xyz="0 0 0" rpy="0 0 0"/>
<ray>
<scan>
<horizontal samples="100" resolution="1" min_angle="-1.5708" max_angle="1.5708"/>
<vertical samples="1" resolution="1" min_angle="0" max_angle="0"/>
</scan>
</ray>
</sensor>
*/
#ifndef URDF_SENSOR_H
#define URDF_SENSOR_H
#include <string>
#include <vector>
#include <map>
#include "urdf_model/pose.h"
#include "urdf_model/joint.h"
#include "urdf_model/link.h"
#include "urdf_model/types.h"
#include "urdf_sensor/types.h"
namespace urdf{
class VisualSensor
{
public:
enum {CAMERA, RAY} type;
virtual ~VisualSensor(void)
{
}
};
class Camera : public VisualSensor
{
public:
Camera() { this->clear(); };
unsigned int width, height;
/// format is optional: defaults to R8G8B8), but can be
/// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8)
std::string format;
double hfov;
double near;
double far;
void clear()
{
hfov = 0;
width = 0;
height = 0;
format.clear();
near = 0;
far = 0;
};
};
class Ray : public VisualSensor
{
public:
Ray() { this->clear(); };
unsigned int horizontal_samples;
double horizontal_resolution;
double horizontal_min_angle;
double horizontal_max_angle;
unsigned int vertical_samples;
double vertical_resolution;
double vertical_min_angle;
double vertical_max_angle;
void clear()
{
// set defaults
horizontal_samples = 1;
horizontal_resolution = 1;
horizontal_min_angle = 0;
horizontal_max_angle = 0;
vertical_samples = 1;
vertical_resolution = 1;
vertical_min_angle = 0;
vertical_max_angle = 0;
};
};
class Sensor
{
public:
Sensor() { this->clear(); };
/// sensor name must be unique
std::string name;
/// update rate in Hz
double update_rate;
/// transform from parent frame to optical center
/// with z-forward and x-right, y-down
Pose origin;
/// sensor
VisualSensorSharedPtr sensor;
/// Parent link element name. A pointer is stored in parent_link_.
std::string parent_link_name;
LinkSharedPtr getParent() const
{return parent_link_.lock();};
void setParent(LinkSharedPtr parent)
{ this->parent_link_ = parent; }
void clear()
{
this->name.clear();
this->sensor.reset();
this->parent_link_name.clear();
this->parent_link_.reset();
};
private:
LinkWeakPtr parent_link_;
};
}
#endif

52
src/kdl_parser/include/urdf_sensor/types.h
View File

@@ -1,52 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Steve Peters */
#ifndef URDF_SENSOR_TYPES_H
#define URDF_SENSOR_TYPES_H
#include <memory>
namespace urdf{
class VisualSensor;
// typedef shared pointers
typedef std::shared_ptr<VisualSensor> VisualSensorSharedPtr;
}
#endif

52
src/kdl_parser/include/urdf_world/types.h
View File

@@ -1,52 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Steve Peters */
#ifndef URDF_WORLD_TYPES_H
#define URDF_WORLD_TYPES_H
#include <memory>
namespace urdf{
class ModelInterface;
// typedef shared pointers
typedef std::shared_ptr<ModelInterface> ModelInterfaceSharedPtr;
}
#endif

110
src/kdl_parser/include/urdf_world/world.h
View File

@@ -1,110 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
/* encapsulates components in a world
see http://ros.org/wiki/usdf/XML/urdf_world
for details
*/
/* example world XML
<world name="pr2_with_table">
<!-- include the models by including
either the complete urdf or
referencing the file name. -->
<model name="pr2">
...
</model>
<include filename="table.urdf" model_name="table_model"/>
<!-- models in the world -->
<entity model="pr2" name="prj">
<origin xyz="0 1 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
<entity model="pr2" name="prk">
<origin xyz="0 2 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
<entity model="table_model">
<origin xyz="0 3 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
</world>
*/
#ifndef URDF_WORLD_H
#define URDF_WORLD_H
#include <string>
#include <vector>
#include <map>
#include "urdf_model/model.h"
#include "urdf_model/pose.h"
#include "urdf_model/twist.h"
#include "urdf_world/types.h"
namespace urdf{
class Entity
{
public:
ModelInterfaceSharedPtr model;
Pose origin;
Twist twist;
};
class World
{
public:
World() { this->clear(); };
/// world name must be unique
std::string name;
std::vector<Entity> models;
void clear()
{
this->name.clear();
};
};
}
#endif

642
src/kdl_parser/src/joint.cpp
View File

@@ -1,642 +0,0 @@
/*********************************************************************
* Software Ligcense Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
#include <locale>
#include <sstream>
#include <stdexcept>
#include <string>
#include <urdf_model/joint.h>
//#include <console_bridge/console.h>
#include <tinyxml.h>
#include <urdf_parser/urdf_parser.h>
namespace urdf{
bool parsePose(Pose &pose, TiXmlElement* xml);
bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config)
{
jd.clear();
// Get joint damping
const char* damping_str = config->Attribute("damping");
if (damping_str == NULL){
////CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no damping, defaults to 0");
jd.damping = 0;
}
else
{
try {
jd.damping = strToDouble(damping_str);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("damping value (%s) is not a valid float", damping_str);
return false;
}
}
// Get joint friction
const char* friction_str = config->Attribute("friction");
if (friction_str == NULL){
////CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no friction, defaults to 0");
jd.friction = 0;
}
else
{
try {
jd.friction = strToDouble(friction_str);
} catch (std::runtime_error &) {
////CONSOLE_BRIDGE_logError("friction value (%s) is not a valid float", friction_str);
return false;
}
}
if (damping_str == NULL && friction_str == NULL)
{
////CONSOLE_BRIDGE_logError("joint dynamics element specified with no damping and no friction");
return false;
}
else{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction);
return true;
}
}
bool parseJointLimits(JointLimits &jl, TiXmlElement* config)
{
jl.clear();
// Get lower joint limit
const char* lower_str = config->Attribute("lower");
if (lower_str == NULL){
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no lower, defaults to 0");
jl.lower = 0;
}
else
{
try {
jl.lower = strToDouble(lower_str);
} catch (std::runtime_error &) {
////CONSOLE_BRIDGE_logError("lower value (%s) is not a valid float", lower_str);
return false;
}
}
// Get upper joint limit
const char* upper_str = config->Attribute("upper");
if (upper_str == NULL){
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no upper, , defaults to 0");
jl.upper = 0;
}
else
{
try {
jl.upper = strToDouble(upper_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("upper value (%s) is not a valid float", upper_str);
return false;
}
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");
if (effort_str == NULL){
////CONSOLE_BRIDGE_logError("joint limit: no effort");
return false;
}
else
{
try {
jl.effort = strToDouble(effort_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("effort value (%s) is not a valid float", effort_str);
return false;
}
}
// Get joint velocity limit
const char* velocity_str = config->Attribute("velocity");
if (velocity_str == NULL){
////CONSOLE_BRIDGE_logError("joint limit: no velocity");
return false;
}
else
{
try {
jl.velocity = strToDouble(velocity_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("velocity value (%s) is not a valid float", velocity_str);
return false;
}
}
return true;
}
bool parseJointSafety(JointSafety &js, TiXmlElement* config)
{
js.clear();
// Get soft_lower_limit joint limit
const char* soft_lower_limit_str = config->Attribute("soft_lower_limit");
if (soft_lower_limit_str == NULL)
{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value");
js.soft_lower_limit = 0;
}
else
{
try {
js.soft_lower_limit = strToDouble(soft_lower_limit_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("soft_lower_limit value (%s) is not a valid float", soft_lower_limit_str);
return false;
}
}
// Get soft_upper_limit joint limit
const char* soft_upper_limit_str = config->Attribute("soft_upper_limit");
if (soft_upper_limit_str == NULL)
{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value");
js.soft_upper_limit = 0;
}
else
{
try {
js.soft_upper_limit = strToDouble(soft_upper_limit_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("soft_upper_limit value (%s) is not a valid float", soft_upper_limit_str);
return false;
}
}
// Get k_position_ safety "position" gain - not exactly position gain
const char* k_position_str = config->Attribute("k_position");
if (k_position_str == NULL)
{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no k_position, using default value");
js.k_position = 0;
}
else
{
try {
js.k_position = strToDouble(k_position_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("k_position value (%s) is not a valid float", k_position_str);
return false;
}
}
// Get k_velocity_ safety velocity gain
const char* k_velocity_str = config->Attribute("k_velocity");
if (k_velocity_str == NULL)
{
////CONSOLE_BRIDGE_logError("joint safety: no k_velocity");
return false;
}
else
{
try {
js.k_velocity = strToDouble(k_velocity_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("k_velocity value (%s) is not a valid float", k_velocity_str);
return false;
}
}
return true;
}
bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config)
{
jc.clear();
// Get rising edge position
const char* rising_position_str = config->Attribute("rising");
if (rising_position_str == NULL)
{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no rising, using default value");
jc.rising.reset();
}
else
{
try {
jc.rising.reset(new double(strToDouble(rising_position_str)));
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("rising value (%s) is not a valid float", rising_position_str);
return false;
}
}
// Get falling edge position
const char* falling_position_str = config->Attribute("falling");
if (falling_position_str == NULL)
{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no falling, using default value");
jc.falling.reset();
}
else
{
try {
jc.falling.reset(new double(strToDouble(falling_position_str)));
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("falling value (%s) is not a valid float", falling_position_str);
return false;
}
}
return true;
}
bool parseJointMimic(JointMimic &jm, TiXmlElement* config)
{
jm.clear();
// Get name of joint to mimic
const char* joint_name_str = config->Attribute("joint");
if (joint_name_str == NULL)
{
////CONSOLE_BRIDGE_logError("joint mimic: no mimic joint specified");
return false;
}
else
jm.joint_name = joint_name_str;
// Get mimic multiplier
const char* multiplier_str = config->Attribute("multiplier");
if (multiplier_str == NULL)
{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1");
jm.multiplier = 1;
}
else
{
try {
jm.multiplier = strToDouble(multiplier_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("multiplier value (%s) is not a valid float", multiplier_str);
return false;
}
}
// Get mimic offset
const char* offset_str = config->Attribute("offset");
if (offset_str == NULL)
{
//CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no offset, using default value of 0");
jm.offset = 0;
}
else
{
try {
jm.offset = strToDouble(offset_str);
} catch(std::runtime_error &) {
////CONSOLE_BRIDGE_logError("offset value (%s) is not a valid float", offset_str);
return false;
}
}
return true;
}
bool parseJoint(Joint &joint, TiXmlElement* config)
{
joint.clear();
// Get Joint Name
const char *name = config->Attribute("name");
if (!name)
{
////CONSOLE_BRIDGE_logError("unnamed joint found");
return false;
}
joint.name = name;
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
if (!origin_xml)
{
//CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str());
joint.parent_to_joint_origin_transform.clear();
}
else
{
if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml))
{
joint.parent_to_joint_origin_transform.clear();
////CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str());
return false;
}
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
{
//CONSOLE_BRIDGE_logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str());
}
else
{
joint.parent_link_name = std::string(pname);
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
const char *pname = child_xml->Attribute("link");
if (!pname)
{
//CONSOLE_BRIDGE_logInform("no child link name specified for Joint link [%s].", joint.name.c_str());
}
else
{
joint.child_link_name = std::string(pname);
}
}
// Get Joint type
const char* type_char = config->Attribute("type");
if (!type_char)
{
////CONSOLE_BRIDGE_logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str());
return false;
}
std::string type_str = type_char;
if (type_str == "planar")
joint.type = Joint::PLANAR;
else if (type_str == "floating")
joint.type = Joint::FLOATING;
else if (type_str == "revolute")
joint.type = Joint::REVOLUTE;
else if (type_str == "continuous")
joint.type = Joint::CONTINUOUS;
else if (type_str == "prismatic")
joint.type = Joint::PRISMATIC;
else if (type_str == "fixed")
joint.type = Joint::FIXED;
else
{
////CONSOLE_BRIDGE_logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str());
return false;
}
// Get Joint Axis
if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
//CONSOLE_BRIDGE_logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str());
joint.axis = Vector3(1.0, 0.0, 0.0);
}
else{
if (axis_xml->Attribute("xyz")){
try {
joint.axis.init(axis_xml->Attribute("xyz"));
}
catch (ParseError &e) {
joint.axis.clear();
////CONSOLE_BRIDGE_logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what());
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
joint.limits.reset(new JointLimits());
if (!parseJointLimits(*joint.limits, limit_xml))
{
//CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str());
joint.limits.reset();
return false;
}
}
else if (joint.type == Joint::REVOLUTE)
{
//CONSOLE_BRIDGE_logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str());
return false;
}
else if (joint.type == Joint::PRISMATIC)
{
//CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str());
return false;
}
// Get safety
TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
if (safety_xml)
{
joint.safety.reset(new JointSafety());
if (!parseJointSafety(*joint.safety, safety_xml))
{
//CONSOLE_BRIDGE_logError("Could not parse safety element for joint [%s]", joint.name.c_str());
joint.safety.reset();
return false;
}
}
// Get calibration
TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
if (calibration_xml)
{
joint.calibration.reset(new JointCalibration());
if (!parseJointCalibration(*joint.calibration, calibration_xml))
{
//CONSOLE_BRIDGE_logError("Could not parse calibration element for joint [%s]", joint.name.c_str());
joint.calibration.reset();
return false;
}
}
// Get Joint Mimic
TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
if (mimic_xml)
{
joint.mimic.reset(new JointMimic());
if (!parseJointMimic(*joint.mimic, mimic_xml))
{
//CONSOLE_BRIDGE_logError("Could not parse mimic element for joint [%s]", joint.name.c_str());
joint.mimic.reset();
return false;
}
}
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
joint.dynamics.reset(new JointDynamics());
if (!parseJointDynamics(*joint.dynamics, prop_xml))
{
//CONSOLE_BRIDGE_logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str());
joint.dynamics.reset();
return false;
}
}
return true;
}
/* exports */
bool exportPose(Pose &pose, TiXmlElement* xml);
bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml)
{
TiXmlElement *dynamics_xml = new TiXmlElement("dynamics");
dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping) );
dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction) );
xml->LinkEndChild(dynamics_xml);
return true;
}
bool exportJointLimits(JointLimits &jl, TiXmlElement* xml)
{
TiXmlElement *limit_xml = new TiXmlElement("limit");
limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort) );
limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity) );
limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower) );
limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper) );
xml->LinkEndChild(limit_xml);
return true;
}
bool exportJointSafety(JointSafety &js, TiXmlElement* xml)
{
TiXmlElement *safety_xml = new TiXmlElement("safety_controller");
safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position) );
safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity) );
safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit) );
safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit) );
xml->LinkEndChild(safety_xml);
return true;
}
bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml)
{
if (jc.falling || jc.rising)
{
TiXmlElement *calibration_xml = new TiXmlElement("calibration");
if (jc.falling)
calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) );
if (jc.rising)
calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) );
//calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) );
xml->LinkEndChild(calibration_xml);
}
return true;
}
bool exportJointMimic(JointMimic &jm, TiXmlElement* xml)
{
if (!jm.joint_name.empty())
{
TiXmlElement *mimic_xml = new TiXmlElement("mimic");
mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset) );
mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier) );
mimic_xml->SetAttribute("joint", jm.joint_name );
xml->LinkEndChild(mimic_xml);
}
return true;
}
bool exportJoint(Joint &joint, TiXmlElement* xml)
{
TiXmlElement * joint_xml = new TiXmlElement("joint");
joint_xml->SetAttribute("name", joint.name);
if (joint.type == urdf::Joint::PLANAR)
joint_xml->SetAttribute("type", "planar");
else if (joint.type == urdf::Joint::FLOATING)
joint_xml->SetAttribute("type", "floating");
else if (joint.type == urdf::Joint::REVOLUTE)
joint_xml->SetAttribute("type", "revolute");
else if (joint.type == urdf::Joint::CONTINUOUS)
joint_xml->SetAttribute("type", "continuous");
else if (joint.type == urdf::Joint::PRISMATIC)
joint_xml->SetAttribute("type", "prismatic");
else if (joint.type == urdf::Joint::FIXED)
joint_xml->SetAttribute("type", "fixed");
else
//CONSOLE_BRIDGE_logError("ERROR: Joint [%s] type [%d] is not a defined type.\n",joint.name.c_str(), joint.type);
// origin
exportPose(joint.parent_to_joint_origin_transform, joint_xml);
// axis
TiXmlElement * axis_xml = new TiXmlElement("axis");
axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis));
joint_xml->LinkEndChild(axis_xml);
// parent
TiXmlElement * parent_xml = new TiXmlElement("parent");
parent_xml->SetAttribute("link", joint.parent_link_name);
joint_xml->LinkEndChild(parent_xml);
// child
TiXmlElement * child_xml = new TiXmlElement("child");
child_xml->SetAttribute("link", joint.child_link_name);
joint_xml->LinkEndChild(child_xml);
if (joint.dynamics)
exportJointDynamics(*(joint.dynamics), joint_xml);
if (joint.limits)
exportJointLimits(*(joint.limits), joint_xml);
if (joint.safety)
exportJointSafety(*(joint.safety), joint_xml);
if (joint.calibration)
exportJointCalibration(*(joint.calibration), joint_xml);
if (joint.mimic)
exportJointMimic(*(joint.mimic), joint_xml);
xml->LinkEndChild(joint_xml);
return true;
}
}

261
src/kdl_parser/src/kdl_parser.cpp
View File

@@ -1,261 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#include "kdl_parser/kdl_parser.hpp"
#include <string>
#include <vector>
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <kdl/frames_io.hpp>
//#ifdef HAS_ROS
//#include <ros/console.h>
//#else
// forward ROS warnings and errors to stderr
#define ROS_DEBUG(...) fprintf(stdout, __VA_ARGS__);
#define ROS_ERROR(...) fprintf(stderr, __VA_ARGS__);
#define ROS_WARN(...) fprintf(stderr, __VA_ARGS__);
//#endif
//#ifdef HAS_URDF
#include <urdf_model/model.h>
//#include <urdf_model/urdfdom_compatibility.h>
//#endif
namespace kdl_parser
{
// construct vector
KDL::Vector toKdl(urdf::Vector3 v)
{
return KDL::Vector(v.x, v.y, v.z);
}
// construct rotation
KDL::Rotation toKdl(urdf::Rotation r)
{
return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w);
}
// construct pose
KDL::Frame toKdl(urdf::Pose p)
{
return KDL::Frame(toKdl(p.rotation), toKdl(p.position));
}
// construct joint
KDL::Joint toKdl(urdf::JointSharedPtr jnt)
{
KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
switch (jnt->type) {
case urdf::Joint::FIXED: {
return KDL::Joint(jnt->name, KDL::Joint::None);
}
case urdf::Joint::REVOLUTE: {
KDL::Vector axis = toKdl(jnt->axis);
return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
}
case urdf::Joint::CONTINUOUS: {
KDL::Vector axis = toKdl(jnt->axis);
return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
}
case urdf::Joint::PRISMATIC: {
KDL::Vector axis = toKdl(jnt->axis);
return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis);
}
default: {
ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
return KDL::Joint(jnt->name, KDL::Joint::None);
}
}
return KDL::Joint();
}
// construct inertia
KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
{
KDL::Frame origin = toKdl(i->origin);
// the mass is frame independent
double kdl_mass = i->mass;
// kdl and urdf both specify the com position in the reference frame of the link
KDL::Vector kdl_com = origin.p;
// kdl specifies the inertia matrix in the reference frame of the link,
// while the urdf specifies the inertia matrix in the inertia reference frame
KDL::RotationalInertia urdf_inertia =
KDL::RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
// Rotation operators are not defined for rotational inertia,
// so we use the RigidBodyInertia operators (with com = 0) as a workaround
KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround =
origin.M * KDL::RigidBodyInertia(0, KDL::Vector::Zero(), urdf_inertia);
// Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
// while the getRotationalInertia method returns the 3d inertia wrt the frame origin
// (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
KDL::RotationalInertia kdl_inertia_wrt_com =
kdl_inertia_wrt_com_workaround.getRotationalInertia();
return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com);
}
// recursive function to walk through tree
bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree)
{
std::vector<urdf::LinkSharedPtr> children = root->child_links;
ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size());
// constructs the optional inertia
KDL::RigidBodyInertia inert(0);
if (root->inertial) {
inert = toKdl(root->inertial);
}
// constructs the kdl joint
KDL::Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
KDL::Segment sgm(root->name, jnt, toKdl(
root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i = 0; i < children.size(); i++) {
if (!addChildrenToTree(children[i], tree)) {
return false;
}
}
return true;
}
bool treeFromFileDocument(const std::string & file, KDL::Tree & tree)
{
const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFileDocument(file);
return kdl_parser::treeFromUrdfModel(*robot_model, tree);
}
bool treeFromFile(const std::string& file, KDL::Tree& tree)
{
const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(file);
return kdl_parser::treeFromUrdfModel(*robot_model, tree);
}
bool treeFromParam(const std::string & param, KDL::Tree & tree)
{
#if defined(HAS_ROS) && defined(HAS_URDF)
urdf::Model robot_model;
if (!robot_model.initParam(param)){
ROS_ERROR("Could not generate robot model");
return false;
}
return treeFromUrdfModel(robot_model, tree);
#else
return false;
#endif
}
bool treeFromString(const std::string & xml, KDL::Tree & tree)
{
const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDF(xml);
if (!robot_model) {
ROS_ERROR("Could not generate robot model");
return false;
}
return kdl_parser::treeFromUrdfModel(*robot_model, tree);
}
bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree)
{
if (!xml_doc) {
ROS_ERROR("Could not parse the xml document");
return false;
}
tinyxml2::XMLPrinter printer;
xml_doc->Print(&printer);
return treeFromString(printer.CStr(), tree);
}
bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
{
if (!xml_doc) {
ROS_ERROR("Could not parse the xml document");
return false;
}
std::stringstream ss;
ss << *xml_doc;
return treeFromString(ss.str(), tree);
}
bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)
{
if (!robot_model.getRoot()) {
return false;
}
tree = KDL::Tree(robot_model.getRoot()->name);
// warn if root link has inertia. KDL does not support this
if (robot_model.getRoot()->inertial) {
ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not "
"support a root link with an inertia. As a workaround, you can add an extra "
"dummy link to your URDF.", robot_model.getRoot()->name.c_str());
}
// add all children
for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) {
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) {
return false;
}
}
return true;
}
} // namespace kdl_parser

670
src/kdl_parser/src/link.cpp
View File

@@ -1,670 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#include <urdf_parser/urdf_parser.h>
#include <urdf_model/link.h>
#include <fstream>
#include <locale>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include <algorithm>
#include <tinyxml.h>
//#include <console_bridge/console.h>
namespace urdf{
bool parsePose(Pose &pose, TiXmlElement* xml);
bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok)
{
bool has_rgb = false;
bool has_filename = false;
material.clear();
if (!config->Attribute("name"))
{
//CONSOLE_BRIDGE_logError("Material must contain a name attribute");
return false;
}
material.name = config->Attribute("name");
// texture
TiXmlElement *t = config->FirstChildElement("texture");
if (t)
{
if (t->Attribute("filename"))
{
material.texture_filename = t->Attribute("filename");
has_filename = true;
}
}
// color
TiXmlElement *c = config->FirstChildElement("color");
if (c)
{
if (c->Attribute("rgba")) {
try {
material.color.init(c->Attribute("rgba"));
has_rgb = true;
}
catch (ParseError &e) {
material.color.clear();
//CONSOLE_BRIDGE_logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str());
}
}
}
if (!has_rgb && !has_filename) {
if (!only_name_is_ok) // no need for an error if only name is ok
{
if (!has_rgb) //CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] color has no rgba").c_str());
if (!has_filename){ //CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] not defined in file").c_str());
}
}
return false;
}
return true;
}
bool parseSphere(Sphere &s, TiXmlElement *c)
{
s.clear();
s.type = Geometry::SPHERE;
if (!c->Attribute("radius"))
{
//CONSOLE_BRIDGE_logError("Sphere shape must have a radius attribute");
return false;
}
try {
s.radius = strToDouble(c->Attribute("radius"));
} catch(std::runtime_error &) {
std::stringstream stm;
stm << "radius [" << c->Attribute("radius") << "] is not a valid float";
//CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
return true;
}
bool parseBox(Box &b, TiXmlElement *c)
{
b.clear();
b.type = Geometry::BOX;
if (!c->Attribute("size"))
{
//CONSOLE_BRIDGE_logError("Box shape has no size attribute");
return false;
}
try
{
b.dim.init(c->Attribute("size"));
}
catch (ParseError &e)
{
b.dim.clear();
//CONSOLE_BRIDGE_logError(e.what());
return false;
}
return true;
}
bool parseCylinder(Cylinder &y, TiXmlElement *c)
{
y.clear();
y.type = Geometry::CYLINDER;
if (!c->Attribute("length") ||
!c->Attribute("radius"))
{
//CONSOLE_BRIDGE_logError("Cylinder shape must have both length and radius attributes");
return false;
}
try {
y.length = strToDouble(c->Attribute("length"));
} catch(std::runtime_error &) {
std::stringstream stm;
stm << "length [" << c->Attribute("length") << "] is not a valid float";
//CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
try {
y.radius = strToDouble(c->Attribute("radius"));
} catch(std::runtime_error &) {
std::stringstream stm;
stm << "radius [" << c->Attribute("radius") << "] is not a valid float";
//CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
return true;
}
bool parseMesh(Mesh &m, TiXmlElement *c)
{
m.clear();
m.type = Geometry::MESH;
if (!c->Attribute("filename")) {
//CONSOLE_BRIDGE_logError("Mesh must contain a filename attribute");
return false;
}
m.filename = c->Attribute("filename");
if (c->Attribute("scale")) {
try {
m.scale.init(c->Attribute("scale"));
}
catch (ParseError &e) {
m.scale.clear();
//CONSOLE_BRIDGE_logError("Mesh scale was specified, but could not be parsed: %s", e.what());
return false;
}
}
else
{
m.scale.x = m.scale.y = m.scale.z = 1;
}
return true;
}
GeometrySharedPtr parseGeometry(TiXmlElement *g)
{
GeometrySharedPtr geom;
if (!g) return geom;
TiXmlElement *shape = g->FirstChildElement();
if (!shape)
{
//CONSOLE_BRIDGE_logError("Geometry tag contains no child element.");
return geom;
}
std::string type_name = shape->ValueStr();
if (type_name == "sphere")
{
Sphere *s = new Sphere();
geom.reset(s);
if (parseSphere(*s, shape))
return geom;
}
else if (type_name == "box")
{
Box *b = new Box();
geom.reset(b);
if (parseBox(*b, shape))
return geom;
}
else if (type_name == "cylinder")
{
Cylinder *c = new Cylinder();
geom.reset(c);
if (parseCylinder(*c, shape))
return geom;
}
else if (type_name == "mesh")
{
Mesh *m = new Mesh();
geom.reset(m);
if (parseMesh(*m, shape))
return geom;
}
else
{
//CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str());
return geom;
}
return GeometrySharedPtr();
}
bool parseInertial(Inertial &i, TiXmlElement *config)
{
i.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(i.origin, o))
return false;
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
//CONSOLE_BRIDGE_logError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
//CONSOLE_BRIDGE_logError("Inertial: mass element must have value attribute");
return false;
}
try {
i.mass = strToDouble(mass_xml->Attribute("value"));
} catch(std::runtime_error &) {
std::stringstream stm;
stm << "Inertial: mass [" << mass_xml->Attribute("value")
<< "] is not a float";
//CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
//CONSOLE_BRIDGE_logError("Inertial element must have inertia element");
return false;
}
std::vector<std::pair<std::string, double>> attrs{
std::make_pair("ixx", 0.0),
std::make_pair("ixy", 0.0),
std::make_pair("ixz", 0.0),
std::make_pair("iyy", 0.0),
std::make_pair("iyz", 0.0),
std::make_pair("izz", 0.0)
};
for (auto& attr : attrs)
{
if (!inertia_xml->Attribute(attr.first))
{
std::stringstream stm;
stm << "Inertial: inertia element missing " << attr.first << " attribute";
//CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
try {
attr.second = strToDouble(inertia_xml->Attribute(attr.first.c_str()));
} catch(std::runtime_error &) {
std::stringstream stm;
stm << "Inertial: inertia element " << attr.first << " is not a valid double";
//CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
}
i.ixx = attrs[0].second;
i.ixy = attrs[1].second;
i.ixz = attrs[2].second;
i.iyy = attrs[3].second;
i.iyz = attrs[4].second;
i.izz = attrs[5].second;
return true;
}
bool parseVisual(Visual &vis, TiXmlElement *config)
{
vis.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o) {
if (!parsePose(vis.origin, o))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
vis.geometry = parseGeometry(geom);
if (!vis.geometry)
return false;
const char *name_char = config->Attribute("name");
if (name_char)
vis.name = name_char;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat) {
// get material name
if (!mat->Attribute("name")) {
//CONSOLE_BRIDGE_logError("Visual material must contain a name attribute");
return false;
}
vis.material_name = mat->Attribute("name");
// try to parse material element in place
vis.material.reset(new Material());
if (!parseMaterial(*vis.material, mat, true))
{
vis.material.reset();
}
}
return true;
}
bool parseCollision(Collision &col, TiXmlElement* config)
{
col.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o) {
if (!parsePose(col.origin, o))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
col.geometry = parseGeometry(geom);
if (!col.geometry)
return false;
const char *name_char = config->Attribute("name");
if (name_char)
col.name = name_char;
return true;
}
bool parseLink(Link &link, TiXmlElement* config)
{
link.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
//CONSOLE_BRIDGE_logError("No name given for the link.");
return false;
}
link.name = std::string(name_char);
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
link.inertial.reset(new Inertial());
if (!parseInertial(*link.inertial, i))
{
//CONSOLE_BRIDGE_logError("Could not parse inertial element for Link [%s]", link.name.c_str());
return false;
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
VisualSharedPtr vis;
vis.reset(new Visual());
if (parseVisual(*vis, vis_xml))
{
link.visual_array.push_back(vis);
}
else
{
vis.reset();
//CONSOLE_BRIDGE_logError("Could not parse visual element for Link [%s]", link.name.c_str());
return false;
}
}
// Visual (optional)
// Assign the first visual to the .visual ptr, if it exists
if (!link.visual_array.empty())
link.visual = link.visual_array[0];
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
CollisionSharedPtr col;
col.reset(new Collision());
if (parseCollision(*col, col_xml))
{
link.collision_array.push_back(col);
}
else
{
col.reset();
//CONSOLE_BRIDGE_logError("Could not parse collision element for Link [%s]", link.name.c_str());
return false;
}
}
// Collision (optional)
// Assign the first collision to the .collision ptr, if it exists
if (!link.collision_array.empty())
link.collision = link.collision_array[0];
return true;
}
/* exports */
bool exportPose(Pose &pose, TiXmlElement* xml);
bool exportMaterial(Material &material, TiXmlElement *xml)
{
TiXmlElement *material_xml = new TiXmlElement("material");
material_xml->SetAttribute("name", material.name);
TiXmlElement* texture = new TiXmlElement("texture");
if (!material.texture_filename.empty())
texture->SetAttribute("filename", material.texture_filename);
material_xml->LinkEndChild(texture);
TiXmlElement* color = new TiXmlElement("color");
color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color));
material_xml->LinkEndChild(color);
xml->LinkEndChild(material_xml);
return true;
}
bool exportSphere(Sphere &s, TiXmlElement *xml)
{
// e.g. add <sphere radius="1"/>
TiXmlElement *sphere_xml = new TiXmlElement("sphere");
sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius));
xml->LinkEndChild(sphere_xml);
return true;
}
bool exportBox(Box &b, TiXmlElement *xml)
{
// e.g. add <box size="1 1 1"/>
TiXmlElement *box_xml = new TiXmlElement("box");
box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim));
xml->LinkEndChild(box_xml);
return true;
}
bool exportCylinder(Cylinder &y, TiXmlElement *xml)
{
// e.g. add <cylinder radius="1"/>
TiXmlElement *cylinder_xml = new TiXmlElement("cylinder");
cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius));
cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length));
xml->LinkEndChild(cylinder_xml);
return true;
}
bool exportMesh(Mesh &m, TiXmlElement *xml)
{
// e.g. add <mesh filename="my_file" scale="1 1 1"/>
TiXmlElement *mesh_xml = new TiXmlElement("mesh");
if (!m.filename.empty())
mesh_xml->SetAttribute("filename", m.filename);
mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale));
xml->LinkEndChild(mesh_xml);
return true;
}
bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml)
{
TiXmlElement *geometry_xml = new TiXmlElement("geometry");
if (urdf::dynamic_pointer_cast<Sphere>(geom))
{
exportSphere((*(urdf::dynamic_pointer_cast<Sphere>(geom).get())), geometry_xml);
}
else if (urdf::dynamic_pointer_cast<Box>(geom))
{
exportBox((*(urdf::dynamic_pointer_cast<Box>(geom).get())), geometry_xml);
}
else if (urdf::dynamic_pointer_cast<Cylinder>(geom))
{
exportCylinder((*(urdf::dynamic_pointer_cast<Cylinder>(geom).get())), geometry_xml);
}
else if (urdf::dynamic_pointer_cast<Mesh>(geom))
{
exportMesh((*(urdf::dynamic_pointer_cast<Mesh>(geom).get())), geometry_xml);
}
else
{
//CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!");
Sphere *s = new Sphere();
s->radius = 0.03;
geom.reset(s);
exportSphere((*(urdf::dynamic_pointer_cast<Sphere>(geom).get())), geometry_xml);
}
xml->LinkEndChild(geometry_xml);
return true;
}
bool exportInertial(Inertial &i, TiXmlElement *xml)
{
// adds <inertial>
// <mass value="1"/>
// <pose xyz="0 0 0" rpy="0 0 0"/>
// <inertia ixx="1" ixy="0" />
// </inertial>
TiXmlElement *inertial_xml = new TiXmlElement("inertial");
TiXmlElement *mass_xml = new TiXmlElement("mass");
mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass));
inertial_xml->LinkEndChild(mass_xml);
exportPose(i.origin, inertial_xml);
TiXmlElement *inertia_xml = new TiXmlElement("inertia");
inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx));
inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy));
inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz));
inertia_xml->SetAttribute("iyy", urdf_export_helpers::values2str(i.iyy));
inertia_xml->SetAttribute("iyz", urdf_export_helpers::values2str(i.iyz));
inertia_xml->SetAttribute("izz", urdf_export_helpers::values2str(i.izz));
inertial_xml->LinkEndChild(inertia_xml);
xml->LinkEndChild(inertial_xml);
return true;
}
bool exportVisual(Visual &vis, TiXmlElement *xml)
{
// <visual group="default">
// <origin rpy="0 0 0" xyz="0 0 0"/>
// <geometry>
// <mesh filename="mesh.dae"/>
// </geometry>
// <material name="Grey"/>
// </visual>
TiXmlElement * visual_xml = new TiXmlElement("visual");
exportPose(vis.origin, visual_xml);
exportGeometry(vis.geometry, visual_xml);
if (vis.material)
exportMaterial(*vis.material, visual_xml);
xml->LinkEndChild(visual_xml);
return true;
}
bool exportCollision(Collision &col, TiXmlElement* xml)
{
// <collision group="default">
// <origin rpy="0 0 0" xyz="0 0 0"/>
// <geometry>
// <mesh filename="mesh.dae"/>
// </geometry>
// <material name="Grey"/>
// </collision>
TiXmlElement * collision_xml = new TiXmlElement("collision");
exportPose(col.origin, collision_xml);
exportGeometry(col.geometry, collision_xml);
xml->LinkEndChild(collision_xml);
return true;
}
bool exportLink(Link &link, TiXmlElement* xml)
{
TiXmlElement * link_xml = new TiXmlElement("link");
link_xml->SetAttribute("name", link.name);
if (link.inertial)
exportInertial(*link.inertial, link_xml);
for (std::size_t i = 0 ; i < link.visual_array.size() ; ++i)
exportVisual(*link.visual_array[i], link_xml);
for (std::size_t i = 0 ; i < link.collision_array.size() ; ++i)
exportCollision(*link.collision_array[i], link_xml);
xml->LinkEndChild(link_xml);
return true;
}
}

326
src/kdl_parser/src/model.cpp
View File

@@ -1,326 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#include <fstream>
#include <map>
#include <stdexcept>
#include <string>
#include "urdf_parser/urdf_parser.h"
//#include <console_bridge/console.h>
namespace urdf{
bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok);
bool parseLink(Link &link, TiXmlElement *config);
bool parseJoint(Joint &joint, TiXmlElement *config);
ModelInterfaceSharedPtr parseURDFFileDocument(const std::string & xml_str)
{
//std::ifstream stream( path.c_str() );
//if (!stream)
//{
// ////CONSOLE_BRIDGE_logError(("File " + path + " does not exist").c_str());
// return ModelInterfaceSharedPtr();
//}
//std::string xml_str((std::istreambuf_iterator<char>(stream)),
// std::istreambuf_iterator<char>());
return urdf::parseURDF( xml_str );
}
ModelInterfaceSharedPtr parseURDFFile(const std::string& path)
{
std::ifstream stream( path.c_str() );
if (!stream)
{
////CONSOLE_BRIDGE_logError(("File " + path + " does not exist").c_str());
return ModelInterfaceSharedPtr();
}
std::string xml_str((std::istreambuf_iterator<char>(stream)),
std::istreambuf_iterator<char>());
return urdf::parseURDF(xml_str);
}
bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& model, const char* link_name)
{
if (visual->material_name.empty())
return true;
const MaterialSharedPtr& material = model->getMaterial(visual->material_name);
if (material)
{
//CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link_name, visual->material_name.c_str());
visual->material = material;
}
else
{
if (visual->material)
{
//CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link_name, visual->material_name.c_str());
model->materials_.insert(make_pair(visual->material->name, visual->material));
}
else
{
//CONSOLE_BRIDGE_logWarn("link '%s' material '%s' undefined.", link_name,visual->material_name.c_str());
return false;
}
}
return true;
}
ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
{
ModelInterfaceSharedPtr model(new ModelInterface);
model->clear();
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
if (xml_doc.Error())
{
////CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc());
xml_doc.ClearError();
model.reset();
return model;
}
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
////CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file");
model.reset();
return model;
}
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
////CONSOLE_BRIDGE_logError("No name given for the robot.");
model.reset();
return model;
}
model->name_ = std::string(name);
try
{
urdf_export_helpers::URDFVersion version(robot_xml->Attribute("version"));
if (!version.equal(1, 0))
{
throw std::runtime_error("Invalid 'version' specified; only version 1.0 is currently supported");
}
}
catch (const std::runtime_error & err)
{
////CONSOLE_BRIDGE_logError(err.what());
model.reset();
return model;
}
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
MaterialSharedPtr material;
material.reset(new Material);
try {
parseMaterial(*material, material_xml, false); // material needs to be fully defined here
if (model->getMaterial(material->name))
{
////CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
material.reset();
model.reset();
return model;
}
else
{
model->materials_.insert(make_pair(material->name,material));
//CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
}
}
catch (ParseError &/*e*/) {
////CONSOLE_BRIDGE_logError("material xml is not initialized correctly");
material.reset();
model.reset();
return model;
}
}
// Get all Link elements
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
LinkSharedPtr link;
link.reset(new Link);
try {
parseLink(*link, link_xml);
if (model->getLink(link->name))
{
////CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
model.reset();
return model;
}
else
{
// set link visual(s) material
//CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
if (link->visual)
{
assignMaterial(link->visual, model, link->name.c_str());
}
for (const auto& visual : link->visual_array)
{
assignMaterial(visual, model, link->name.c_str());
}
model->links_.insert(make_pair(link->name,link));
//CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
}
}
catch (ParseError &/*e*/) {
////CONSOLE_BRIDGE_logError("link xml is not initialized correctly");
model.reset();
return model;
}
}
if (model->links_.empty()){
////CONSOLE_BRIDGE_logError("No link elements found in urdf file");
model.reset();
return model;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
JointSharedPtr joint;
joint.reset(new Joint);
if (parseJoint(*joint, joint_xml))
{
if (model->getJoint(joint->name))
{
////CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
model.reset();
return model;
}
else
{
model->joints_.insert(make_pair(joint->name,joint));
//CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
}
}
else
{
////CONSOLE_BRIDGE_logError("joint xml is not initialized correctly");
model.reset();
return model;
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
try
{
model->initTree(parent_link_tree);
}
catch(ParseError &e)
{
////CONSOLE_BRIDGE_logError("Failed to build tree: %s", e.what());
model.reset();
return model;
}
// find the root link
try
{
model->initRoot(parent_link_tree);
}
catch(ParseError &e)
{
////CONSOLE_BRIDGE_logError("Failed to find root link: %s", e.what());
model.reset();
return model;
}
return model;
}
bool exportMaterial(Material &material, TiXmlElement *config);
bool exportLink(Link &link, TiXmlElement *config);
bool exportJoint(Joint &joint, TiXmlElement *config);
TiXmlDocument* exportURDF(const ModelInterface &model)
{
TiXmlDocument *doc = new TiXmlDocument();
TiXmlElement *robot = new TiXmlElement("robot");
robot->SetAttribute("name", model.name_);
doc->LinkEndChild(robot);
for (std::map<std::string, MaterialSharedPtr>::const_iterator m=model.materials_.begin(); m!=model.materials_.end(); m++)
{
//CONSOLE_BRIDGE_logDebug("urdfdom: exporting material [%s]\n",m->second->name.c_str());
exportMaterial(*(m->second), robot);
}
for (std::map<std::string, LinkSharedPtr>::const_iterator l=model.links_.begin(); l!=model.links_.end(); l++)
{
//CONSOLE_BRIDGE_logDebug("urdfdom: exporting link [%s]\n",l->second->name.c_str());
exportLink(*(l->second), robot);
}
for (std::map<std::string, JointSharedPtr>::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); j++)
{
//CONSOLE_BRIDGE_logDebug("urdfdom: exporting joint [%s]\n",j->second->name.c_str());
exportJoint(*(j->second), robot);
}
return doc;
}
TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model)
{
return exportURDF(*model);
}
}

135
src/kdl_parser/src/pose.cpp
View File

@@ -1,135 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen, John Hsu */
#include <urdf_model/pose.h>
#include <fstream>
#include <sstream>
#include <algorithm>
//#include <console_bridge/console.h>
#include <tinyxml.h>
#include <urdf_parser/urdf_parser.h>
namespace urdf_export_helpers {
std::string values2str(unsigned int count, const double *values, double (*conv)(double))
{
std::stringstream ss;
for (unsigned int i = 0 ; i < count ; i++)
{
if (i > 0)
ss << " ";
ss << (conv ? conv(values[i]) : values[i]);
}
return ss.str();
}
std::string values2str(urdf::Vector3 vec)
{
double xyz[3];
xyz[0] = vec.x;
xyz[1] = vec.y;
xyz[2] = vec.z;
return values2str(3, xyz);
}
std::string values2str(urdf::Rotation rot)
{
double rpy[3];
rot.getRPY(rpy[0], rpy[1], rpy[2]);
return values2str(3, rpy);
}
std::string values2str(urdf::Color c)
{
double rgba[4];
rgba[0] = c.r;
rgba[1] = c.g;
rgba[2] = c.b;
rgba[3] = c.a;
return values2str(4, rgba);
}
std::string values2str(double d)
{
return values2str(1, &d);
}
}
namespace urdf{
bool parsePose(Pose &pose, TiXmlElement* xml)
{
pose.clear();
if (xml)
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str != NULL)
{
try {
pose.position.init(xyz_str);
}
catch (ParseError &e) {
////CONSOLE_BRIDGE_logError(e.what());
return false;
}
}
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str != NULL)
{
try {
pose.rotation.init(rpy_str);
}
catch (ParseError &e) {
////CONSOLE_BRIDGE_logError(e.what());
return false;
}
}
}
return true;
}
bool exportPose(Pose &pose, TiXmlElement* xml)
{
TiXmlElement *origin = new TiXmlElement("origin");
std::string pose_xyz_str = urdf_export_helpers::values2str(pose.position);
std::string pose_rpy_str = urdf_export_helpers::values2str(pose.rotation);
origin->SetAttribute("xyz", pose_xyz_str);
origin->SetAttribute("rpy", pose_rpy_str);
xml->LinkEndChild(origin);
return true;
}
}

111
src/kdl_parser/src/tinystr.cpp
View File

@@ -1,111 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#ifndef TIXML_USE_STL
#include "tinystr.h"
// Error value for find primitive
const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
// Null rep.
TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
void TiXmlString::reserve (size_type cap)
{
if (cap > capacity())
{
TiXmlString tmp;
tmp.init(length(), cap);
memcpy(tmp.start(), data(), length());
swap(tmp);
}
}
TiXmlString& TiXmlString::assign(const char* str, size_type len)
{
size_type cap = capacity();
if (len > cap || cap > 3*(len + 8))
{
TiXmlString tmp;
tmp.init(len);
memcpy(tmp.start(), str, len);
swap(tmp);
}
else
{
memmove(start(), str, len);
set_size(len);
}
return *this;
}
TiXmlString& TiXmlString::append(const char* str, size_type len)
{
size_type newsize = length() + len;
if (newsize > capacity())
{
reserve (newsize + capacity());
}
memmove(finish(), str, len);
set_size(newsize);
return *this;
}
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
{
TiXmlString tmp;
tmp.reserve(a.length() + b.length());
tmp += a;
tmp += b;
return tmp;
}
TiXmlString operator + (const TiXmlString & a, const char* b)
{
TiXmlString tmp;
TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
tmp.reserve(a.length() + b_len);
tmp += a;
tmp.append(b, b_len);
return tmp;
}
TiXmlString operator + (const char* a, const TiXmlString & b)
{
TiXmlString tmp;
TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
tmp.reserve(a_len + b.length());
tmp.append(a, a_len);
tmp += b;
return tmp;
}
#endif // TIXML_USE_STL

1886
src/kdl_parser/src/tinyxml.cpp
View File

File diff suppressed because it is too large Load Diff

2951
src/kdl_parser/src/tinyxml2.cpp
View File

File diff suppressed because it is too large Load Diff

52
src/kdl_parser/src/tinyxmlerror.cpp
View File

@@ -1,52 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#include "tinyxml.h"
// The goal of the seperate error file is to make the first
// step towards localization. tinyxml (currently) only supports
// english error messages, but the could now be translated.
//
// It also cleans up the code a bit.
//
const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
{
"No error",
"Error",
"Failed to open file",
"Error parsing Element.",
"Failed to read Element name",
"Error reading Element value.",
"Error reading Attributes.",
"Error: empty tag.",
"Error reading end tag.",
"Error parsing Unknown.",
"Error parsing Comment.",
"Error parsing Declaration.",
"Error document empty.",
"Error null (0) or unexpected EOF found in input stream.",
"Error parsing CDATA.",
"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
};

1638
src/kdl_parser/src/tinyxmlparser.cpp
View File

File diff suppressed because it is too large Load Diff

84
src/kdl_parser/src/twist.cpp
View File

@@ -1,84 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
#include <urdf_model/twist.h>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <tinyxml.h>
//#include <console_bridge/console.h>
namespace urdf{
bool parseTwist(Twist &twist, TiXmlElement* xml)
{
twist.clear();
if (xml)
{
const char* linear_char = xml->Attribute("linear");
if (linear_char != NULL)
{
try {
twist.linear.init(linear_char);
}
catch (ParseError &e) {
twist.linear.clear();
//CONSOLE_BRIDGE_logError("Malformed linear string [%s]: %s", linear_char, e.what());
return false;
}
}
const char* angular_char = xml->Attribute("angular");
if (angular_char != NULL)
{
try {
twist.angular.init(angular_char);
}
catch (ParseError &e) {
twist.angular.clear();
//CONSOLE_BRIDGE_logError("Malformed angular [%s]: %s", angular_char, e.what());
return false;
}
}
}
return true;
}
}

159
src/kdl_parser/src/urdf_model_state.cpp
View File

@@ -1,159 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
#include <urdf_model_state/model_state.h>
#include <urdf_model/utils.h>
#include <fstream>
#include <locale>
#include <sstream>
#include <stdexcept>
#include <string>
#include <algorithm>
#include <tinyxml.h>
//#include <console_bridge/console.h>
namespace urdf {
bool parseModelState(ModelState& ms, TiXmlElement* config)
{
ms.clear();
const char* name_char = config->Attribute("name");
if (!name_char)
{
/*CONSOLE_BRIDGE_logError("No name given for the model_state.");*/
return false;
}
ms.name = std::string(name_char);
const char* time_stamp_char = config->Attribute("time_stamp");
if (time_stamp_char)
{
try {
ms.time_stamp.set(strToDouble(time_stamp_char));
}
catch (std::runtime_error&) {
//CONSOLE_BRIDGE_logError("Parsing time stamp [%s] failed", time_stamp_char);
return false;
}
}
TiXmlElement* joint_state_elem = config->FirstChildElement("joint_state");
if (joint_state_elem)
{
JointStateSharedPtr joint_state;
joint_state.reset(new JointState());
const char* joint_char = joint_state_elem->Attribute("joint");
if (joint_char)
joint_state->joint = std::string(joint_char);
else
{
//CONSOLE_BRIDGE_logError("No joint name given for the model_state.");
return false;
}
/*// parse position*/
const char* position_char = joint_state_elem->Attribute("position");
if (position_char)
{
std::vector<std::string> pieces;
urdf::split_string(pieces, position_char, " ");
for (unsigned int i = 0; i < pieces.size(); ++i) {
if (pieces[i] != "") {
try {
joint_state->position.push_back(strToDouble(pieces[i].c_str()));
}
catch (std::runtime_error&) {
throw ParseError("position element (" + pieces[i] + ") is not a valid float");
}
}
}
}
/* parse velocity*/
const char* velocity_char = joint_state_elem->Attribute("velocity");
/**/
int i = 0;
if (velocity_char)
{
std::vector<std::string> pieces;
urdf::split_string(pieces, velocity_char, " ");
for (unsigned int i = 0; i < pieces.size(); ++i) {
if (pieces[i] != "") {
try {
joint_state->velocity.push_back(strToDouble(pieces[i].c_str()));
}
catch (std::runtime_error&) {
throw ParseError("velocity element (" + pieces[i] + ") is not a valid float");
}
}
}
}
// parse effort
const char* effort_char = joint_state_elem->Attribute("effort");
/**/
if(effort_char)
{
std::vector<std::string> pieces;
urdf::split_string(pieces, effort_char, " ");
for (unsigned int i = 0; i < pieces.size(); ++i) {
if (pieces[i] != "") {
try {
joint_state->effort.push_back(strToDouble(pieces[i].c_str()));
}
catch (std::runtime_error&) {
throw ParseError("effort element (" + pieces[i] + ") is not a valid float");
}
}
}
}
/*// add to vector*/
ms.joint_states.push_back(joint_state);
/* */
}
return false;
}
}

360
src/kdl_parser/src/urdf_sensor.cpp
View File

@@ -1,360 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: John Hsu */
#include <urdf_sensor/sensor.h>
#include <fstream>
#include <locale>
#include <sstream>
#include <stdexcept>
#include <string>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
namespace urdf{
bool parsePose(Pose &pose, TiXmlElement* xml);
bool parseCamera(Camera &camera, TiXmlElement* config)
{
camera.clear();
camera.type = VisualSensor::CAMERA;
TiXmlElement *image = config->FirstChildElement("image");
if (image)
{
const char* width_char = image->Attribute("width");
if (width_char)
{
try
{
camera.width = std::stoul(width_char);
}
catch (std::invalid_argument &e)
{
//CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what());
return false;
}
catch (std::out_of_range &e)
{
//CONSOLE_BRIDGE_logError("Camera image width [%s] is out of range: %s", width_char, e.what());
return false;
}
}
else
{
//CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute");
return false;
}
const char* height_char = image->Attribute("height");
if (height_char)
{
try
{
camera.height = std::stoul(height_char);
}
catch (std::invalid_argument &e)
{
//CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what());
return false;
}
catch (std::out_of_range &e)
{
//CONSOLE_BRIDGE_logError("Camera image height [%s] is out of range: %s", height_char, e.what());
return false;
}
}
else
{
//CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute");
return false;
}
const char* format_char = image->Attribute("format");
if (format_char)
camera.format = std::string(format_char);
else
{
//CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute");
return false;
}
const char* hfov_char = image->Attribute("hfov");
if (hfov_char)
{
try {
camera.hfov = strToDouble(hfov_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float", hfov_char);
return false;
}
}
else
{
//CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute");
return false;
}
const char* near_char = image->Attribute("near");
if (near_char)
{
try {
camera.near = strToDouble(near_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float", near_char);
return false;
}
}
else
{
//CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute");
return false;
}
const char* far_char = image->Attribute("far");
if (far_char)
{
try {
camera.far = strToDouble(far_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float", far_char);
return false;
}
}
else
{
//CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute");
return false;
}
}
else
{
//CONSOLE_BRIDGE_logError("Camera sensor has no <image> element");
return false;
}
return true;
}
bool parseRay(Ray &ray, TiXmlElement* config)
{
ray.clear();
ray.type = VisualSensor::RAY;
TiXmlElement *horizontal = config->FirstChildElement("horizontal");
if (horizontal)
{
const char* samples_char = horizontal->Attribute("samples");
if (samples_char)
{
try
{
ray.horizontal_samples = std::stoul(samples_char);
}
catch (std::invalid_argument &e)
{
//CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what());
return false;
}
catch (std::out_of_range &e)
{
//CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is out of range: %s", samples_char, e.what());
return false;
}
}
const char* resolution_char = horizontal->Attribute("resolution");
if (resolution_char)
{
try {
ray.horizontal_resolution = strToDouble(resolution_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float", resolution_char);
return false;
}
}
const char* min_angle_char = horizontal->Attribute("min_angle");
if (min_angle_char)
{
try {
ray.horizontal_min_angle = strToDouble(min_angle_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float", min_angle_char);
return false;
}
}
const char* max_angle_char = horizontal->Attribute("max_angle");
if (max_angle_char)
{
try {
ray.horizontal_max_angle = strToDouble(max_angle_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float", max_angle_char);
return false;
}
}
}
TiXmlElement *vertical = config->FirstChildElement("vertical");
if (vertical)
{
const char* samples_char = vertical->Attribute("samples");
if (samples_char)
{
try
{
ray.vertical_samples = std::stoul(samples_char);
}
catch (std::invalid_argument &e)
{
//CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what());
return false;
}
catch (std::out_of_range &e)
{
//CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is out of range: %s", samples_char, e.what());
return false;
}
}
const char* resolution_char = vertical->Attribute("resolution");
if (resolution_char)
{
try {
ray.vertical_resolution = strToDouble(resolution_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float", resolution_char);
return false;
}
}
const char* min_angle_char = vertical->Attribute("min_angle");
if (min_angle_char)
{
try {
ray.vertical_min_angle = strToDouble(min_angle_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float", min_angle_char);
return false;
}
}
const char* max_angle_char = vertical->Attribute("max_angle");
if (max_angle_char)
{
try {
ray.vertical_max_angle = strToDouble(max_angle_char);
} catch(std::runtime_error &) {
//CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float", max_angle_char);
return false;
}
}
}
return false;
}
VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g)
{
VisualSensorSharedPtr visual_sensor;
// get sensor type
TiXmlElement *sensor_xml;
if (g->FirstChildElement("camera"))
{
Camera *camera = new Camera();
visual_sensor.reset(camera);
sensor_xml = g->FirstChildElement("camera");
if (!parseCamera(*camera, sensor_xml))
visual_sensor.reset();
}
else if (g->FirstChildElement("ray"))
{
Ray *ray = new Ray();
visual_sensor.reset(ray);
sensor_xml = g->FirstChildElement("ray");
if (!parseRay(*ray, sensor_xml))
visual_sensor.reset();
}
else
{
//CONSOLE_BRIDGE_logError("No know sensor types [camera|ray] defined in <sensor> block");
}
return visual_sensor;
}
bool parseSensor(Sensor &sensor, TiXmlElement* config)
{
sensor.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
//CONSOLE_BRIDGE_logError("No name given for the sensor.");
return false;
}
sensor.name = std::string(name_char);
// parse parent_link_name
const char *parent_link_name_char = config->Attribute("parent_link_name");
if (!parent_link_name_char)
{
//CONSOLE_BRIDGE_logError("No parent_link_name given for the sensor.");
return false;
}
sensor.parent_link_name = std::string(parent_link_name_char);
// parse origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(sensor.origin, o))
return false;
}
// parse sensor
sensor.sensor = parseVisualSensor(config);
return true;
}
}

70
src/kdl_parser/src/world.cpp
View File

@@ -1,70 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: Wim Meeussen */
#include <urdf_world/world.h>
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <tinyxml.h>
//#include <console_bridge/console.h>
namespace urdf{
bool parseWorld(World &/*world*/, TiXmlElement* /*config*/)
{
// to be implemented
return true;
}
bool exportWorld(World &world, TiXmlElement* xml)
{
TiXmlElement * world_xml = new TiXmlElement("world");
world_xml->SetAttribute("name", world.name);
// to be implemented
// exportModels(*world.models, world_xml);
xml->LinkEndChild(world_xml);
return true;
}
}

30
src/math_utils.h
View File

@@ -1,30 +0,0 @@
#ifndef MATH_UTILS_H
#define MATH_UTILS_H
#ifdef __cplusplus
extern "C"
{
#endif
// 基础数学运算
int add(int a, int b);
int subtract(int a, int b);
float multiply(float a, float b);
float divide(float a, float b);
int fibonacci(int n);
// 内存操作
void *create_buffer(int size);
void destroy_buffer(void *p);
// 数组运算
int compute_sum(int *arr, int size);
// 字符串操作
const char *get_greeting();
#ifdef __cplusplus
}
#endif
#endif // MATH_UTILS_H

100
src/smart_json_wrapper.h
View File

@@ -1,100 +0,0 @@
#ifndef SMART_JSON_WRAPPER_H
#define SMART_JSON_WRAPPER_H
#include "function_metadata.h"
#include "math_utils.h"
#include <string>
#include <map>
#include <any>
#include <chrono>
#include <sstream>
#include <regex>
#include <cstring>
// JSON 响应结构
struct SmartResponse
{
bool success = true;
int code = 0;
std::string msg;
std::string req_code;
std::string req_cmd;
std::any res_data;
double execution_time = 0.0;
std::map<std::string, std::string> execution_info;
// 转换为 JSON 字符串
std::string toJson() const;
// 创建成功响应
static SmartResponse createSuccess(const std::string &req_code,
const std::string &req_cmd,
const std::any &data = {});
// 创建错误响应
static SmartResponse createError(const std::string &req_code,
const std::string &req_cmd,
int code,
const std::string &msg);
};
// 智能 JSON 处理器
class SmartJsonProcessor
{
private:
// 注册所有数学函数
void registerMathFunctions();
// 智能类型推断
std::any inferJsonValue(const std::string &key, const std::string &value_str);
// 解析数组
std::vector<int> parseIntArray(const std::string &array_str);
std::vector<float> parseFloatArray(const std::string &array_str);
// 字符串处理
static std::string trim(const std::string &str);
static std::vector<std::string> split(const std::string &str, char delimiter);
public:
SmartJsonProcessor();
~SmartJsonProcessor();
// JSON 解析
std::map<std::string, std::any> parseJsonParams(const std::string &json_str);
// 处理 JSON 请求
SmartResponse processRequest(const std::string &json_request);
// 获取函数列表
std::map<std::string, std::shared_ptr<FunctionInfo>> getAvailableFunctions() const;
// 获取函数信息
std::string getFunctionInfo(const std::string &func_name) const;
};
// WASM 导出函数
extern "C"
{
const char *init_func();
// 智能处理 JSON 请求
const char *func(const char *json_request);
// 智能处理 JSON 请求
const char *smart_process_json(const char *json_request);
// 获取可用函数列表
const char *smart_get_function_list();
// 获取函数详情
const char *smart_get_function_info(const char *func_name);
// 测试智能匹配
const char *smart_test_match(const char *json_request);
// 释放字符串
void smart_free_string(const char *str);
// 获取版本信息
const char *smart_get_version();
}
#endif // SMART_JSON_WRAPPER_H