1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115 | /**
* @file MotionStatus.h
*
* @author <a href="mailto:krause@informatik.hu-berlin.de">Thomas Krause</a>
* @author <a href="mailto:xu@informatik.hu-berlin.de">Xu, Yuan</a>
* Definition of the class MotionStatus
*/
#ifndef __MotionStatus_h_
#define __MotionStatus_h_
#include "Tools/Math/Vector2.h"
#include "Tools/Math/Pose2D.h"
#include "Tools/DataStructures/Printable.h"
#include "Request/MotionID.h"
#include "Request/HeadMotionRequest.h"
#include <string>
/**
* This describes the MotionStatus
*/
class MotionStatus : public naoth::Printable
{
public:
struct PlannedMotion
{
Pose2D hip;
Pose2D lFoot;
Pose2D rFoot;
};
struct StepControlStatus
{
StepControlStatus() : stepID(0), stepRequestID(0), moveableFoot(NONE) {}
enum MoveableFoot
{
NONE,
LEFT,
RIGHT,
BOTH
};
unsigned int stepID; // the walk only accept request with this id
unsigned int stepRequestID; // for PathPlanner
MoveableFoot moveableFoot; // which foot can be controlled
};
/** constructor */
MotionStatus()<--- Member variable 'MotionStatus::head_at_rest' is not initialized in the constructor.
:
time(0),
lastMotion(motion::num_of_motions),
currentMotion(motion::num_of_motions),
currentMotionState(motion::stopped),
headMotion(HeadMotionRequest::numOfHeadMotion),
target_reached(false),
head_target_reached(false),
head_got_stuck(false),
walk_emergency_stop(false)
{
}
~MotionStatus(){}
// TODO: add comments!!!
unsigned int time;
motion::MotionID lastMotion;
motion::MotionID currentMotion;
motion::State currentMotionState;
HeadMotionRequest::HeadMotionID headMotion;
PlannedMotion plannedMotion;
StepControlStatus stepControl;
bool target_reached; //TODO: which motion do set that? Maybe better stand_target reached
bool head_target_reached;
bool head_at_rest;
// deprecated: head_got_stuck can be calculated as: !head_target_reached && head_at_rest
bool head_got_stuck;
bool walk_emergency_stop;
virtual void print(std::ostream& stream) const
{
stream << "time = " << time << '\n';
stream << "lastMotion = " << motion::getName(lastMotion) << '\n';
stream << "currentMotion = " << motion::getName(currentMotion) << '\n';
stream << "currentMotionState = " << motion::getName(currentMotionState) << '\n';
stream << "plannedMotion: \n";
stream << "hip = "<< plannedMotion.hip <<"\n";
stream << "left foot = "<< plannedMotion.lFoot<<"\n";
stream << "right foot = "<< plannedMotion.rFoot<<"\n";
stream << "step control = "<< stepControl.stepID << " " << stepControl.moveableFoot <<"\n";
stream << "target_reached = " << target_reached << '\n';
stream << "head_target_reached = " << head_target_reached << '\n';
stream << "head_at_rest = " << head_at_rest << '\n';
stream << "head_got_stuck = " << head_got_stuck << '\n';
stream << "walk_emergency_stop = " << walk_emergency_stop << '\n';
}//end print
};
namespace naoth
{
template<>
class Serializer<MotionStatus>
{
public:
static void serialize(const MotionStatus& representation, std::ostream& stream);
static void deserialize(std::istream& stream, MotionStatus& representation);
};
}
#endif // __MotionStatus_h_
|