95 lines
2.9 KiB
C++
95 lines
2.9 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef WALKMODULEABSOLUTEHEADINGCONTROL_H
|
||
#define WALKMODULEABSOLUTEHEADINGCONTROL_H
|
||
|
||
/**
|
||
* compare the state's absolute heading against a given compass [control]
|
||
*/
|
||
|
||
#include "WalkModule.h"
|
||
#include "WalkStateHeading.h"
|
||
|
||
#include "../../../../geo/Heading.h"
|
||
#include "../../../../math/Distributions.h"
|
||
|
||
|
||
/** keep the state's heading */
|
||
template <typename Node, typename WalkState, typename Control> class WalkModuleAbsoluteHeadingControl : public WalkModule<Node, WalkState> {
|
||
|
||
const float sigma_rad;
|
||
const Control* ctrl;
|
||
|
||
public:
|
||
|
||
/** ctor. 180 should be OK! */
|
||
WalkModuleAbsoluteHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
|
||
sigma_rad(Angle::degToRad(sensorNoiseDegreesSigma)),
|
||
ctrl(ctrl) {
|
||
|
||
// ensure the template WalkState inherits from 'WalkStateHeading'!
|
||
StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
|
||
|
||
}
|
||
|
||
|
||
virtual void updateBefore(WalkState& state, const Node& startNode) override {
|
||
(void) state;
|
||
(void) startNode;
|
||
}
|
||
|
||
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
||
(void) state;
|
||
(void) startNode;
|
||
(void) endNode;
|
||
}
|
||
|
||
|
||
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
||
(void) state;
|
||
(void) curNode;
|
||
(void) nextNode;
|
||
}
|
||
|
||
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
||
|
||
(void) startNode;
|
||
|
||
// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
|
||
// reset this one after every transition!
|
||
Assert::isBetween(ctrl->compassAzimuth_rad, 0.0f, (float)(2*M_PI), "the given absolute heading is out of bounds");
|
||
|
||
// ignore for stairs?
|
||
//if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;}
|
||
|
||
// for elevator edges [same (x,y) but different z] just return 1
|
||
if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
|
||
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
|
||
//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
|
||
|
||
// get the heading between curNode and potentialNode
|
||
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
|
||
|
||
// compare the heading against the state's heading - the last error
|
||
const Heading stateHead = state.heading.direction;
|
||
|
||
// get the difference
|
||
const float angularDiff = head.getDiffHalfRAD(stateHead);
|
||
|
||
if (angularDiff > Angle::degToRad(100)) {return 0.10;}
|
||
{return 0.90;}
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // WALKMODULEABSOLUTEHEADINGCONTROL_H
|