112 lines
3.4 KiB
C++
112 lines
3.4 KiB
C++
#ifndef WALKMODULEACTIVITYCONTROL_H
|
|
#define WALKMODULEACTIVITYCONTROL_H
|
|
|
|
#include "WalkModule.h"
|
|
|
|
#include "../../../../geo/Heading.h"
|
|
#include "../../../../math/distribution/Normal.h"
|
|
|
|
#include "../../../../sensors/activity/ActivityButterPressure.h"
|
|
|
|
|
|
/**
|
|
* use the currently detected activity (stay-on-floor, walk-up, walk-down, ..)
|
|
* from the system's control data to favor edges that resemble this activity
|
|
*/
|
|
template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControl : public WalkModule<Node, WalkState> {
|
|
|
|
private:
|
|
|
|
const Control* ctrl;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
WalkModuleActivityControl(const Control* ctrl) : ctrl(ctrl) {
|
|
;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
// static double getProbability(const Node& lastNode, const Node& nextNode, const ActivityButterPressure::Activity activity) {
|
|
|
|
// switch (activity) {
|
|
// case ActivityButterPressure::Activity::DOWN:
|
|
// if (deltaZ_cm < 0) {return 0.85;}
|
|
// if (deltaZ_cm == 0) {return 0.10;}
|
|
// {return 0.05;}
|
|
// case ActivityButterPressure::Activity::UP:
|
|
// if (deltaZ_cm > 0) {return 0.85;}
|
|
// if (deltaZ_cm == 0) {return 0.10;}
|
|
// {return 0.05;}
|
|
// case ActivityButterPressure::Activity::STAY:
|
|
// if (deltaZ_cm == 0) {return 0.85;}
|
|
// {return 0.15;}
|
|
// default:
|
|
// throw Exception("not yet implemented");
|
|
// }
|
|
|
|
// return 1.0;
|
|
|
|
// }
|
|
|
|
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
|
|
|
(void) state;
|
|
(void) startNode;
|
|
|
|
const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;
|
|
|
|
// TODO: general activity enum and activity-detector based on barometer and accelerometer?
|
|
const Activity activity = ctrl->activity;
|
|
|
|
// const float kappa = 0.75;
|
|
|
|
switch (activity) {
|
|
case Activity::WALKING_DOWN:
|
|
if (deltaZ_cm < 0) {return 0.60;}
|
|
if (deltaZ_cm == 0) {return 0.25;}
|
|
{return 0.15;}
|
|
case Activity::WALKING_UP:
|
|
if (deltaZ_cm > 0) {return 0.60;}
|
|
if (deltaZ_cm == 0) {return 0.25;}
|
|
{return 0.15;}
|
|
case Activity::WALKING:
|
|
if (deltaZ_cm == 0) {return 0.60;}
|
|
{return 0.40;}
|
|
// case ActivityButterPressure::Activity::DOWN:
|
|
// case ActivityButterPressure::Activity::UP:
|
|
// if (potentialNode.getType() == GridNode::TYPE_STAIR) {return kappa;}
|
|
// if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
|
|
// {return 1-kappa;}
|
|
// case ActivityButterPressure::Activity::STAY:
|
|
// if (potentialNode.getType() == GridNode::TYPE_DOOR) {return kappa;}
|
|
// if (potentialNode.getType() == GridNode::TYPE_FLOOR) {return kappa;}
|
|
// {return 1-kappa;}
|
|
default:
|
|
throw Exception("not yet implemented activity within WalkModuleActivityControl::getProbability");
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // WALKMODULEACTIVITYCONTROL_H
|