Commit 7af37e45 authored by michaero's avatar michaero
Browse files

reworked UWB Service Client

parent 17aed950
......@@ -34,5 +34,5 @@ namespace d_fall_pps
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
// Callback to update anchor Positions
void updateAnchors(const std_msgs::Int32 &message);
void updateCallback(const std_msgs::Int32 &message);
}
\ No newline at end of file
......@@ -20,8 +20,14 @@
#include <string>
#include <vector>
#define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1
#define UWB_UPDATE_ANCHORS 5
using namespace d_fall_pps;
bool enableUWB;
UWBAnchorArray anchors;
ros::NodeHandle nodeHandle("~");
......@@ -32,9 +38,9 @@ int main(int argc, char* argv[])
readYaml();
ros::ServiceServer anchorService = nodeHandle.advertiseService("getAnchors", getAnchorPositions);
ros::ServiceServer uwbService = nodeHandle.advertiseService("UWBServiceClient", getAnchorPositions);
ros::Subscriber anchorUpdate = nodeHandle.subscribe("/my_GUI/anchorUpdate", 1, updateAnchors);
ros::Subscriber uwbUpdate = nodeHandle.subscribe("/my_GUI/UWBUpdate", 1, updateCallback);
ros::spin();
......@@ -83,12 +89,26 @@ void d_fall_pps::readYaml()
bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response& response)
{
response.enableUWB = enableUWB;
response.anchorArray = anchors;
return true;
}
void d_fall_pps::updateAnchors(const std_msgs::Int32 &message)
void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
{
readYaml;
switch(message.data)
{
case UWB_UPDATE_DISABLE:
enableUWB = false;
break;
case UWB_UPDATE_ENABLE:
enableUWB = true;
case UWB_UPDATE_ANCHORS:
readYaml();
break;
default:
ROS_WARN("[UWBServiceClient] unknown update command");
}
ROS_ERROR("it works!!!");
}
---
bool enableUWB
UWBAnchorArray anchorArray
\ 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