-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
lcm_publisher_system.cc
136 lines (113 loc) · 4.64 KB
/
lcm_publisher_system.cc
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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
#include "drake/systems/lcm/lcm_publisher_system.h"
#include <cstdint>
#include <utility>
#include <vector>
#include "drake/common/text_logging.h"
#include "drake/lcm/drake_lcm.h"
#include "drake/lcm/drake_lcm_interface.h"
namespace drake {
namespace systems {
namespace lcm {
using drake::lcm::DrakeLcmInterface;
using drake::lcm::DrakeLcm;
using systems::TriggerTypeSet;
LcmPublisherSystem::LcmPublisherSystem(
const std::string& channel,
std::unique_ptr<SerializerInterface> serializer,
DrakeLcmInterface* lcm,
const TriggerTypeSet& publish_triggers,
double publish_period)
: channel_(channel),
serializer_(std::move(serializer)),
owned_lcm_(lcm ? nullptr : new DrakeLcm()),
lcm_(lcm ? lcm : owned_lcm_.get()),
publish_period_(publish_period) {
DRAKE_DEMAND(serializer_ != nullptr);
DRAKE_DEMAND(lcm_ != nullptr);
DRAKE_DEMAND(publish_period >= 0.0);
DRAKE_DEMAND(!publish_triggers.empty());
// Check that publish_triggers does not contain an unsupported trigger.
for (const auto& trigger : publish_triggers) {
DRAKE_THROW_UNLESS((trigger == TriggerType::kForced) ||
(trigger == TriggerType::kPeriodic) ||
(trigger == TriggerType::kPerStep));
}
// Declare a forced publish so that any time Publish(.) is called on this
// system (or a Diagram containing it), a message is emitted.
if (publish_triggers.find(TriggerType::kForced) != publish_triggers.end()) {
this->DeclareForcedPublishEvent(
&LcmPublisherSystem::PublishInputAsLcmMessage);
}
DeclareAbstractInputPort("lcm_message", *serializer_->CreateDefaultValue());
set_name(make_name(channel_));
if (publish_triggers.find(TriggerType::kPeriodic) != publish_triggers.end()) {
DRAKE_THROW_UNLESS(publish_period > 0.0);
const double offset = 0.0;
this->DeclarePeriodicPublishEvent(
publish_period, offset,
&LcmPublisherSystem::PublishInputAsLcmMessage);
} else {
// publish_period > 0 without TriggerType::kPeriodic has no meaning and is
// likely a mistake.
DRAKE_THROW_UNLESS(publish_period == 0.0);
}
if (publish_triggers.find(TriggerType::kPerStep) != publish_triggers.end()) {
this->DeclarePerStepEvent(
systems::PublishEvent<double>([this](
const systems::Context<double>& context,
const systems::PublishEvent<double>&) {
this->PublishInputAsLcmMessage(context);
}));
}
}
LcmPublisherSystem::LcmPublisherSystem(
const std::string& channel,
std::unique_ptr<SerializerInterface> serializer,
DrakeLcmInterface* lcm, double publish_period)
: LcmPublisherSystem(channel, std::move(serializer), lcm,
(publish_period > 0.0) ?
TriggerTypeSet({TriggerType::kForced, TriggerType::kPeriodic}) :
TriggerTypeSet({TriggerType::kForced, TriggerType::kPerStep}),
publish_period) {}
LcmPublisherSystem::~LcmPublisherSystem() {}
void LcmPublisherSystem::AddInitializationMessage(
InitializationPublisher initialization_publisher) {
DRAKE_DEMAND(initialization_publisher != nullptr);
initialization_publisher_ = std::move(initialization_publisher);
DeclareInitializationEvent(systems::PublishEvent<double>(
TriggerType::kInitialization,
[this](const systems::Context<double>& context,
const systems::PublishEvent<double>&) {
this->initialization_publisher_(context, this->lcm_);
}));
}
std::string LcmPublisherSystem::make_name(const std::string& channel) {
return "LcmPublisherSystem(" + channel + ")";
}
const std::string& LcmPublisherSystem::get_channel_name() const {
return channel_;
}
double LcmPublisherSystem::get_publish_period() const {
return publish_period_;
}
// Takes the VectorBase from the input port of the context and publishes
// it onto an LCM channel. This function is called automatically, as
// necessary, at the requisite publishing period (if a positive publish period
// was passed to the constructor) or per a simulation step (if no publish
// period or publish period = 0.0 was passed to the constructor).
EventStatus LcmPublisherSystem::PublishInputAsLcmMessage(
const Context<double>& context) const {
DRAKE_LOGGER_TRACE("Publishing LCM {} message", channel_);
DRAKE_ASSERT(serializer_ != nullptr);
// Converts the input into LCM message bytes.
const AbstractValue& input = get_input_port().Eval<AbstractValue>(context);
std::vector<uint8_t> message_bytes;
serializer_->Serialize(input, &message_bytes);
// Publishes onto the specified LCM channel.
lcm_->Publish(channel_, message_bytes.data(), message_bytes.size(),
context.get_time());
return EventStatus::Succeeded();
}
} // namespace lcm
} // namespace systems
} // namespace drake