Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixes IgnMonitor bug. #759

Merged
merged 1 commit into from
Mar 5, 2021
Merged

Fixes IgnMonitor bug. #759

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
82 changes: 41 additions & 41 deletions test/regression/cpp/agent_simulation_builder_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -199,22 +199,22 @@ TEST_F(AgentSimulationTest, TestPriusSimpleCar) {
// Set up a monitor to check for ignition::msgs::AgentState
// messages coming from the agent.
const std::string kStateTopicName{"/agents/state"};
test::IgnMonitor<ignition::msgs::AgentState_V> ign_monitor(kStateTopicName);
auto ign_monitor = test::MakeSharedIgnMonitor<ignition::msgs::AgentState_V>(kStateTopicName);

// Shortly after starting, we should not have moved much.
const int kStateMessagesCount{1};
EXPECT_TRUE(ign_monitor.do_until(kStateMessagesCount, kTimeoutMs,
[this, &simulation]() { simulation->StepBy(kSmallTimeStep); }));
EXPECT_TRUE(ign_monitor->do_until(kStateMessagesCount, kTimeoutMs,
[this, &simulation]() { simulation->StepBy(kSmallTimeStep); }));

EXPECT_TRUE(ign_monitor.get_last_message().states_size() > 0);
EXPECT_TRUE(ign_monitor->get_last_message().states_size() > 0);

ignition::msgs::AgentState state_message = ign_monitor.get_last_message().states(0);
ignition::msgs::AgentState state_message = ign_monitor->get_last_message().states(0);
EXPECT_LT(state_message.position().x(), 0.1);

// Move a lot. Confirm that we're moving in +x.
simulation->StepBy(kLargeTimeStep);

state_message = ign_monitor.get_last_message().states(0);
state_message = ign_monitor->get_last_message().states(0);
EXPECT_GT(state_message.position().x(), 1.0);
}

Expand Down Expand Up @@ -243,21 +243,21 @@ TEST_F(AgentSimulationTest, TestPriusUnicycleCar) {
// Set up a monitor to check for ignition::msgs::AgentState
// messages coming from the agent.
const std::string kStateTopicName{"agents/state"};
test::IgnMonitor<ignition::msgs::AgentState_V> ign_monitor(kStateTopicName);
auto ign_monitor = test::MakeSharedIgnMonitor<ignition::msgs::AgentState_V>(kStateTopicName);

const int kStateMessagesCount{1};
EXPECT_TRUE(ign_monitor.do_until(kStateMessagesCount, kTimeoutMs,
[this, &simulation]() { simulation->StepBy(kSmallTimeStep); }));
EXPECT_TRUE(ign_monitor->do_until(kStateMessagesCount, kTimeoutMs,
[this, &simulation]() { simulation->StepBy(kSmallTimeStep); }));

EXPECT_TRUE(ign_monitor.get_last_message().states_size() > 0);
EXPECT_TRUE(ign_monitor->get_last_message().states_size() > 0);

ignition::msgs::AgentState state_message = ign_monitor.get_last_message().states(0);
ignition::msgs::AgentState state_message = ign_monitor->get_last_message().states(0);
EXPECT_LT(state_message.position().x(), 0.1);

// Move a lot. Confirm that we're moving in +x.
simulation->StepBy(kLargeTimeStep);

state_message = ign_monitor.get_last_message().states(0);
state_message = ign_monitor->get_last_message().states(0);
EXPECT_GT(state_message.position().x(), 1.0);
}

Expand All @@ -278,15 +278,15 @@ TEST_F(AgentSimulationTest, TestPriusSimpleCarInitialState) {
// Set up a monitor to check for ignition::msgs::AgentState
// messages coming from the agent.
const std::string kStateTopicName{"agents/state"};
test::IgnMonitor<ignition::msgs::AgentState_V> ign_monitor(kStateTopicName);
auto ign_monitor = test::MakeSharedIgnMonitor<ignition::msgs::AgentState_V>(kStateTopicName);

const int kStateMessagesCount{1};
EXPECT_TRUE(ign_monitor.do_until(kStateMessagesCount, kTimeoutMs,
[this, &simulation]() { simulation->StepBy(kSmallTimeStep); }));
EXPECT_TRUE(ign_monitor->do_until(kStateMessagesCount, kTimeoutMs,
[this, &simulation]() { simulation->StepBy(kSmallTimeStep); }));

EXPECT_TRUE(ign_monitor.get_last_message().states_size() > 0);
EXPECT_TRUE(ign_monitor->get_last_message().states_size() > 0);

const ignition::msgs::AgentState state_message = ign_monitor.get_last_message().states(0);
const ignition::msgs::AgentState state_message = ign_monitor->get_last_message().states(0);

// Computations of AgentState from a PoseBundle incur minimal numerical
// precision loss. Hence, a small tolerance is allowed when comparing the
Expand Down Expand Up @@ -358,20 +358,20 @@ TEST_F(AgentSimulationTest, TestMobilControlledSimpleCar) {
// the visualizer.
const std::string kDrawTopicName{"visualizer/scene_update"};
const std::string kPoseTopicName{"visualizer/pose_update"};
test::IgnMonitor<ignition::msgs::Model_V> ign_monitor(kDrawTopicName);
test::IgnMonitor<ignition::msgs::Pose_V> ign_pose_monitor(kPoseTopicName);
auto ign_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Model_V>(kDrawTopicName);
auto ign_pose_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Pose_V>(kPoseTopicName);

// Advances the simulation to allow the MaliputRailcar to begin
// accelerating.
simulation->StepBy(kLargeTimeStep);

// Ensures that at least one draw message has arrived.
const int kDrawMessagesCount{1};
EXPECT_TRUE(ign_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));

const ignition::msgs::Model_V draw_message = ign_monitor.get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor.get_last_message();
const ignition::msgs::Model_V draw_message = ign_monitor->get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor->get_last_message();
EXPECT_EQ(GetLinkCount(draw_message), 3 * GetPriusLinkCount());

// Expect the SimpleCar to start steering to the left; y value increases.
Expand Down Expand Up @@ -405,19 +405,19 @@ TEST_F(AgentSimulationTest, TestTrajectoryAgent) {
// the visualizer.
const std::string kDrawTopicName{"visualizer/scene_update"};
const std::string kPoseTopicName{"visualizer/pose_update"};
test::IgnMonitor<ignition::msgs::Model_V> ign_monitor(kDrawTopicName);
test::IgnMonitor<ignition::msgs::Pose_V> ign_pose_monitor(kPoseTopicName);
auto ign_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Model_V>(kDrawTopicName);
auto ign_pose_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Pose_V>(kPoseTopicName);

// Simulate for 10 seconds.
simulation->StepBy(10.);

// Ensures at least one draw message has arrived.
const int kDrawMessagesCount{1};
EXPECT_TRUE(ign_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));

const ignition::msgs::Model_V draw_message = ign_monitor.get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor.get_last_message();
const ignition::msgs::Model_V draw_message = ign_monitor->get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor->get_last_message();

CheckModelLinks(draw_message);

Expand Down Expand Up @@ -494,19 +494,19 @@ TEST_F(AgentSimulationTest, TestMaliputRailcar) {
// the visualizer.
const std::string kDrawTopicName{"visualizer/scene_update"};
const std::string kPoseTopicName{"visualizer/pose_update"};
test::IgnMonitor<ignition::msgs::Model_V> ign_monitor(kDrawTopicName);
test::IgnMonitor<ignition::msgs::Pose_V> ign_pose_monitor(kPoseTopicName);
auto ign_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Model_V>(kDrawTopicName);
auto ign_pose_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Pose_V>(kPoseTopicName);

simulation->StepBy(kLargeTimeStep);

// Ensures that at least one draw message has arrived.
const int kDrawMessagesCount{1};
EXPECT_TRUE(ign_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));

// Retrieves last draw message.
const ignition::msgs::Model_V draw_message = ign_monitor.get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor.get_last_message();
const ignition::msgs::Model_V draw_message = ign_monitor->get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor->get_last_message();

// Verifies the acceleration is zero.
CheckModelLinks(draw_message);
Expand Down Expand Up @@ -534,8 +534,8 @@ TEST_F(AgentSimulationTest, TestLcmOutput) {
// the visualizer.
const std::string kDrawTopicName{"visualizer/scene_update"};
const std::string kPoseTopicName{"visualizer/pose_update"};
test::IgnMonitor<ignition::msgs::Model_V> ign_monitor(kDrawTopicName);
test::IgnMonitor<ignition::msgs::Pose_V> ign_pose_monitor(kPoseTopicName);
auto ign_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Model_V>(kDrawTopicName);
auto ign_pose_monitor = test::MakeSharedIgnMonitor<ignition::msgs::Pose_V>(kPoseTopicName);

const std::unique_ptr<const ignition::msgs::Scene> scene = simulation->GetVisualScene();

Expand All @@ -553,12 +553,12 @@ TEST_F(AgentSimulationTest, TestLcmOutput) {
// further changed.
simulation->StepBy(kLargeTimeStep);
const int kDrawMessagesCount{2};
EXPECT_TRUE(ign_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor.wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));
EXPECT_TRUE(ign_pose_monitor->wait_until(kDrawMessagesCount, kTimeoutMs));

// Retrieves last draw message.
const ignition::msgs::Model_V draw_message = ign_monitor.get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor.get_last_message();
const ignition::msgs::Model_V draw_message = ign_monitor->get_last_message();
const ignition::msgs::Pose_V pose_message = ign_pose_monitor->get_last_message();

// Checks number of links in the viewer_draw message.
EXPECT_EQ(GetLinkCount(draw_message), 2 * GetPriusLinkCount());
Expand Down
51 changes: 49 additions & 2 deletions test/regression/cpp/test_utilities/helpers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@
#include <ignition/msgs.hh>
#include <ignition/transport.hh>

#include "delphyne/macros.h"

#define EXPECT_THROW_OF_TYPE(type, statement, msg) \
do { \
try { \
Expand Down Expand Up @@ -153,14 +155,30 @@ ::testing::AssertionResult CheckProtobufMsgEquality(const google::protobuf::Mess
// A helper class to monitor publications over an ignition
// transport topic.
//
// Example to get an instance of `IgnMonitor`:
// @code{.cpp}
// auto ign_monitor = MakeSharedIgnMonitor<ignition::msgs::AgentState_V>(TopicName);
// @endcode
// Then all the public methods could be called
//
// @note: This class subscribes a callback method to a topic. Ignition-transport7 doesn't guarantee that the callback
// method is executed while the instance of the class is still alive. To solve that lack of syncronization we introduce
// a std::share_ptr of the IgnMonitor class into the callback method.
// @remarks DO NOT COPY this implementation in production code. It will prevent an
// instance of this class to be properly released.
// TODO(#760) Restore previous implementation of IgnMonitor.
//
// @tparam IGN_TYPE A valid ignition message type.
template <typename IGN_TYPE,
typename std::enable_if<std::is_base_of<ignition::transport::ProtoMsg, IGN_TYPE>::value, int>::type = 0>
class IgnMonitor {
public:
// Constructs a monitor for the given topic.
// Creates an IgnMonitor.
// @tparam IGN_TYPE A valid ignition message type.
// @param topic_name Valid ignition transport topic name.
explicit IgnMonitor(const std::string& topic_name) { node_.Subscribe(topic_name, &IgnMonitor::OnTopicMessage, this); }
// @returns An instance of IgnMonitor wrapped by std::shared_ptr.
template <typename IGN_TYPE_F>
friend std::shared_ptr<IgnMonitor<IGN_TYPE_F>> MakeSharedIgnMonitor(const std::string& topic_name);

~IgnMonitor() {}

Expand Down Expand Up @@ -208,6 +226,22 @@ class IgnMonitor {
}

private:
// Constructs a IgnMonitor object.
// @param topic_name Valid ignition transport topic name.
explicit IgnMonitor(const std::string& topic_name) : topic_name_(topic_name) {}

// Subscribes to the ignition transport topic.
// @param A pointer to this instance.
// @throws std::logic_error When `self_ptr.get()` != `this`.
void Initialize(std::shared_ptr<IgnMonitor<IGN_TYPE>> self_ptr) {
DELPHYNE_VALIDATE(self_ptr.get() == this, std::logic_error, "self_ptr must point to `this` instance.");
// A std::shared_ptr to `this` is passed to a instance variable
// in order to guarantee that the IgnMonitor instance hasn't been destroyed at
// the moment that the callback method is executed. See #760.
self_ptr_ = self_ptr;
node_.Subscribe(topic_name_, &IgnMonitor::OnTopicMessage, this);
}

// Ignition subscriber callback, updating state
// and notify all threads waiting on this instance.
//
Expand All @@ -219,6 +253,12 @@ class IgnMonitor {
cv_.notify_all();
}

// Topic name.
const std::string topic_name_;
// Holds a pointer to this instance.
// This will cause that the instance of the class won't be deallocated by the SO
// until the execution of the unit is finished. See #760.
std::shared_ptr<IgnMonitor<IGN_TYPE>> self_ptr_{nullptr};
// Received message count.
int message_count_{0};
// Last ignition message received.
Expand All @@ -236,6 +276,13 @@ class IgnMonitor {
ignition::transport::Node node_{};
};

template <typename IGN_TYPE_F>
std::shared_ptr<IgnMonitor<IGN_TYPE_F>> MakeSharedIgnMonitor(const std::string& topic_name) {
auto ign_monitor = std::shared_ptr<IgnMonitor<IGN_TYPE_F>>(new IgnMonitor<IGN_TYPE_F>(topic_name));
ign_monitor->Initialize(ign_monitor);
return ign_monitor;
};

// Makes and returns a temporary directory out of a @p template_path,
// whose last six (6) characters MUST be 'XXXXXX'.
// @see mkdtemp
Expand Down