To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit aac7f101 authored by tiagos's avatar tiagos
Browse files

Self Loc and overall workflow is really pretty now

parent fc10c3c2
......@@ -32,6 +32,7 @@ public:
void setOffset(double x, double y, double z);
bool startAnchorLocalistaion();
private:
Serial serial;
std::list<anchor_raw_data> anchors_raw;
......@@ -48,4 +49,6 @@ private:
bool _startSerialCommunication();
bool _calculateAnchorPosition();
void _writeToYAML();
};
\ No newline at end of file
......@@ -26,10 +26,12 @@ DataListener::~DataListener()
void DataListener::setOffset(double x, double y, double z)
{
printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
offset.x = x;
offset.y = y;
offset.z = z;
//printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
offset.x += x;
offset.y += y;
offset.z += z;
_writeToYAML();
}
void DataListener::_LOC_Debugger(std::string value)
......@@ -141,19 +143,7 @@ bool DataListener::_calculateAnchorPosition()
}
//Save anchor positions to yaml file
ros::NodeHandle nodeHandle("~");
//std::vector<float> anchorArr;
std::vector<double> anchorArr;
for (unsigned int i = 0; i < anchor_position.size(); i++)
{
//printf("anchor%i (%f, %f, %f)\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
anchorArr = {anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z};
nodeHandle.setParam("anchor" + std::to_string(i), anchorArr);
}
_writeToYAML();
finishedLoc = true;
return true;
......@@ -218,4 +208,19 @@ bool DataListener::startAnchorLocalistaion()
return true;
return false;
}
\ No newline at end of file
}
void DataListener::_writeToYAML()
{
ros::NodeHandle nodeHandle("~");
//std::vector<float> anchorArr;
std::vector<double> anchorArr;
for (unsigned int i = 0; i < anchor_position.size(); i++)
{
//printf("anchor%i (%f, %f, %f)\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
anchorArr = {anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z};
nodeHandle.setParam("anchor" + std::to_string(i), anchorArr);
}
}
......@@ -96,7 +96,8 @@ void d_fall_pps::readYaml()
bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response& response)
{
ROS_WARN("x %f, y %f, z %f", request.x, request.y, request.z);
selfLoc.setOffset(-request.x, -request.y, -request.z);
readYaml();
response.enableUWB = enableUWB;
response.anchorArray = anchors;
......
Markdown is supported
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