Commit 1ffcda7e authored by michaero's avatar michaero
Browse files

Get UWB Anchor Positions in a yaml file

parent 5b91247b
pps_ws/build/
pps_ws/devel/
.vscode/
*.pyc
*.orig
......
// ROS node that provides the anchor positions from the yaml file
// Copyright (C) 2017 Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "ros/ros.h"
#include "d_fall_pps/UWBAnchor.h"
#include "d_fall_pps/UWBAnchorArray.h"
namespace d_fall_pps
{
// Callback to return anchor Positions
void getAnchorPositions(UWBAnchorArray &retAnchors);
// Reads anchor positions from yaml file
void readYaml();
// gets float vector from yaml file
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
}
\ No newline at end of file
anchorLength: 3 # Update this number after adding/removing anchors!!!
# Structure: anchorID [x, y, z] (ID is always from 0 to length-1)
anchor0: [0.0, 0.0, 0.0]
anchor1: [0.0, 1.0, 0.0]
anchor2: [1.0, 0.0, 0.0]
// ROS node that provides the anchor positions from the yaml file
// Copyright (C) 2017 Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "AnchorPositions.h"
#include <ros/package.h>
#include <string>
#include <vector>
using namespace d_fall_pps;
UWBAnchorArray anchors;
ros::NodeHandle nodeHandle("~");
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBDataPublisher");
ros::NodeHandle nodeHandle("~");
readYaml();
ros::ServiceServer anchorService = nodeHandle.advertiseService("getAnchors", getAnchorPositions);
ros.spin();
return 0;
}
void d_fall_pps::loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'");
}
if(val.size() != length) {
ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
}
}
void d_fall_pps::readYaml()
{
int length;
if(!nodeHandle.getParam("anchorLength", length))
{
ROS_ERROR("Failed to get Anchors");
return;
}
anchors.length = length;
std::vector<float> anchorArr;
UWBAnchor tempAnchor;
for(int i = 0; i < length; i++)
{
loadParameterFloatVector(nodeHandle, "anchor" + std::to_string(i), anchorArr, 3);
tempAnchor.id = i;
tempAnchor.x = anchorArr[0];
tempAnchor.y = anchorArr[1];
tempAnchor.z = anchorArr[2];
anchors.data.push_back(tempAnchor);
}
return;
}
void d_fall_pps::getAnchorPositions(UWBAnchorArray &retAnchors)
{
retAnchors = anchors;
return;
}
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment