-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
optitrack_receiver.h
53 lines (44 loc) · 1.48 KB
/
optitrack_receiver.h
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
#pragma once
#include <map>
#include <string>
#include <vector>
#include "drake/common/drake_deprecated.h"
#include "drake/math/rigid_transform.h"
#include "drake/systems/framework/leaf_system.h"
namespace drake {
namespace systems {
namespace sensors {
/** Converts LCM optitrack_frame_t message data on an input port to
RigidTransform data on per-body output ports.
@system
name: OptitrackReceiver
input_ports:
- optitrack_frame_t
output_ports:
- <em style="color:gray">...</em>
- <em style="color:gray">...</em>
@endsystem
*/
class DRAKE_DEPRECATED(
"2023-11-01",
"The OptitrackReceiver will be removed from Drake. If you still need to "
"use this class, feel free to copy its source code to your own project "
"and customize it to your needs.") // BR
OptitrackReceiver : public LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OptitrackReceiver)
/** Constructs an OptitrackReceiver.
@param frame_map Mapping from optitrack body id to output port name.
All ids must always be present in the input message.
@param X_WO Pose of the optitrack frame O in the world frame W.
Defaults to the Identity transform.
*/
explicit OptitrackReceiver(const std::map<int, std::string>& frame_map,
const math::RigidTransformd& X_WO = {});
private:
void CalcOutput(const Context<double>&, int, math::RigidTransformd*) const;
const math::RigidTransformd X_WO_;
};
} // namespace sensors
} // namespace systems
} // namespace drake