94 lines
3.4 KiB
C++
94 lines
3.4 KiB
C++
//
|
|
// Created by bastian on 28.02.22.
|
|
//
|
|
|
|
#include "Area.h"
|
|
#include <geometry_msgs/msg/pose.hpp>
|
|
|
|
namespace BT {
|
|
template<>
|
|
std::shared_ptr<Position2D> convertFromString(StringView str) {
|
|
auto split = splitString(str, ',');
|
|
if (split.size() != 2) {
|
|
throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
|
|
}
|
|
|
|
auto pos = std::make_shared<Position2D>();
|
|
pos->x = convertFromString<double>(split[0]);
|
|
pos->y = convertFromString<double>(split[1]);
|
|
return pos;
|
|
}
|
|
|
|
template<>
|
|
std::shared_ptr<Area> convertFromString(StringView str) {
|
|
auto parts = splitString(str, '|');
|
|
if (parts.size() < 3) {
|
|
throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
|
|
}
|
|
|
|
auto output = std::make_shared<Area>();
|
|
std::vector<Position2D> array(parts.size());
|
|
for (unsigned long i = 0; i < parts.size(); ++i) {
|
|
array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
|
|
}
|
|
output->triangles = array;
|
|
return output;
|
|
}
|
|
|
|
template<>
|
|
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
|
|
auto parts = splitString(str, ',');
|
|
if (!(parts.size() == 3 || parts.size() == 7)) {
|
|
throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
|
|
}
|
|
|
|
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
if(parts.size() == 7){
|
|
pose->orientation.w = convertFromString<double>(parts[3]);
|
|
pose->orientation.x = convertFromString<double>(parts[4]);
|
|
pose->orientation.y = convertFromString<double>(parts[5]);
|
|
pose->orientation.z = convertFromString<double>(parts[6]);
|
|
}else{
|
|
pose->orientation.w = 1;
|
|
pose->orientation.x = 0;
|
|
pose->orientation.y = 0;
|
|
pose->orientation.z = 0;
|
|
}
|
|
pose->position.x = convertFromString<double>(parts[0]);
|
|
pose->position.y = convertFromString<double>(parts[1]);
|
|
pose->position.z = convertFromString<double>(parts[2]);
|
|
|
|
return pose;
|
|
}
|
|
|
|
template<>
|
|
std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
|
|
auto parts = splitString(str, ',');
|
|
if (parts.size() != 3) {
|
|
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
|
|
}
|
|
|
|
auto point = std::make_shared<geometry_msgs::msg::Point>();
|
|
point->x = convertFromString<double>(parts[0]);
|
|
point->y = convertFromString<double>(parts[1]);
|
|
point->z = convertFromString<double>(parts[2]);
|
|
|
|
return point;
|
|
}
|
|
|
|
template<>
|
|
std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
|
|
auto parts = splitString(str, ',');
|
|
if (parts.size() != 4) {
|
|
throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
|
|
}
|
|
|
|
auto point = std::make_shared<geometry_msgs::msg::Quaternion>();
|
|
point->w = convertFromString<double>(parts[0]);
|
|
point->x = convertFromString<double>(parts[1]);
|
|
point->y = convertFromString<double>(parts[2]);
|
|
point->z = convertFromString<double>(parts[3]);
|
|
|
|
return point;
|
|
}
|
|
} |