yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added another string to Pose conversion

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-31 16:33:49 +00:00
parent 4e49ecd156
commit 91d464bf9b
1 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/btree_nodes/src/Extensions.cpp
View File

@ -38,15 +38,22 @@ namespace BT {
template<> template<>
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) { std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
auto parts = splitString(str, ','); auto parts = splitString(str, ',');
if (parts.size() != 3) { if (!(parts.size() == 3 || parts.size() == 7)) {
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'."); 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>(); 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.w = 1;
pose->orientation.x = 0; pose->orientation.x = 0;
pose->orientation.y = 0; pose->orientation.y = 0;
pose->orientation.z = 0; pose->orientation.z = 0;
}
pose->position.x = convertFromString<double>(parts[0]); pose->position.x = convertFromString<double>(parts[0]);
pose->position.y = convertFromString<double>(parts[1]); pose->position.y = convertFromString<double>(parts[1]);
pose->position.z = convertFromString<double>(parts[2]); pose->position.z = convertFromString<double>(parts[2]);