fix a counter mismatch bug in lcm_driven_loop (RobotLocomotion#6824)

* fix a counter mismatch bug in lcm_driven_loop
KyleFromKitware · Aug 11, 2017 · f27795a · f27795a
1 parent bfbc9ce
commit f27795a
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 6 deletions.
diff --git a/drake/systems/lcm/lcm_driven_loop.cc b/drake/systems/lcm/lcm_driven_loop.cc
@@ -35,7 +35,7 @@ LcmDrivenLoop::LcmDrivenLoop(
 }
 
 const AbstractValue& LcmDrivenLoop::WaitForMessage() {
-  message_count_ = driving_sub_.WaitForMessage(message_count_);
+  driving_sub_.WaitForMessage(driving_sub_.GetMessageCount(*sub_context_));
 
   driving_sub_.CalcNextUpdateTime(*sub_context_, sub_events_.get());
 

diff --git a/drake/systems/lcm/lcm_driven_loop.h b/drake/systems/lcm/lcm_driven_loop.h
@@ -193,8 +193,6 @@ class LcmDrivenLoop {
   std::unique_ptr<SystemOutput<double>> sub_output_;
   std::unique_ptr<State<double>> sub_swap_state_;
   std::unique_ptr<CompositeEventCollection<double>> sub_events_;
-
-  int message_count_{0};
 };
 
 }  // namespace lcm

diff --git a/drake/systems/lcm/lcm_subscriber_system.cc b/drake/systems/lcm/lcm_subscriber_system.cc
@@ -127,9 +127,7 @@ void LcmSubscriberSystem::ProcessMessageAndStoreToAbstractState(
       .GetMutableValue<int>() = received_message_count_;
 }
 
-void LcmSubscriberSystem::DoCalcNextUpdateTime(
-    const Context<double>& context,
-    systems::CompositeEventCollection<double>* events, double* time) const {
+int LcmSubscriberSystem::GetMessageCount(const Context<double>& context) const {
   // Gets the last message count from either abstract state or discrete state.
   int last_message_count;
   if (translator_ == nullptr) {
@@ -141,6 +139,13 @@ void LcmSubscriberSystem::DoCalcNextUpdateTime(
     last_message_count = static_cast<int>(
         context.get_discrete_state(kStateIndexMessageCount)->GetAtIndex(0));
   }
+  return last_message_count;
+}
+
+void LcmSubscriberSystem::DoCalcNextUpdateTime(
+    const Context<double>& context,
+    systems::CompositeEventCollection<double>* events, double* time) const {
+  int last_message_count = GetMessageCount(context);
 
   std::unique_lock<std::mutex> lock(received_message_mutex_);
   // Has a new message. Schedule an update event.

diff --git a/drake/systems/lcm/lcm_subscriber_system.h b/drake/systems/lcm/lcm_subscriber_system.h
@@ -133,6 +133,11 @@ class LcmSubscriberSystem : public LeafSystem<double>,
    */
   int WaitForMessage(int old_message_count) const;
 
+  /**
+   * Returns the message counter stored in @p context.
+   */
+  int GetMessageCount(const Context<double>& context) const;
+
  protected:
   void DoCalcNextUpdateTime(const Context<double>& context,
                             systems::CompositeEventCollection<double>* events,