Skip to content

Commit

Permalink
👩‍🌾 3 ➡️ 4 (#648)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <[email protected]>
  • Loading branch information
@chapulina
chapulina authored Feb 25, 2021
2 parents d253d81 + 58ac2b1 commit 02bb83a
Show file tree
Hide file tree
Showing 7 changed files with 479 additions and 26 deletions.
20 changes: 19 additions & 1 deletion src/systems/diff_drive/DiffDrive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,9 @@ class ignition::gazebo::systems::DiffDrivePrivate
/// \brief Diff drive odometry message publisher.
public: transport::Node::Publisher odomPub;

/// \brief Diff drive tf message publisher.
public: transport::Node::Publisher tfPub;

/// \brief Linear velocity limiter.
public: std::unique_ptr<SpeedLimiter> limiterLin;

Expand Down Expand Up @@ -283,6 +286,13 @@ void DiffDrive::Configure(const Entity &_entity,
this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
odomTopic);

std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
"/tf"};
if (_sdf->HasElement("tf_topic"))
tfTopic = _sdf->Get<std::string>("tf_topic");
this->dataPtr->tfPub = this->dataPtr->node.Advertise<msgs::Pose_V>(
tfTopic);

if (_sdf->HasElement("frame_id"))
this->dataPtr->sdfFrameId = _sdf->Get<std::string>("frame_id");

Expand Down Expand Up @@ -506,9 +516,17 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info,
childFrame->add_value(this->sdfChildFrameId);
}

// Construct the Pose_V/tf message and publish it.
msgs::Pose_V tfMsg;
ignition::msgs::Pose *tfMsgPose = nullptr;
tfMsgPose = tfMsg.add_pose();
tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header());
tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position());
tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation());

// Publish the message
// Publish the messages
this->odomPub.Publish(msg);
this->tfPub.Publish(tfMsg);
}

//////////////////////////////////////////////////
Expand Down
16 changes: 16 additions & 0 deletions src/systems/diff_drive/DiffDrive.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,22 @@ namespace systems
/// `<odom_topic>`: Custom topic on which this system will publish odometry
/// messages. This element if optional, and the default value is
/// `/model/{name_of_model}/odometry`.
///
/// `<tf_topic>`: Custom topic on which this system will publish the
/// transform from `frame_id` to `child_frame_id`. This element if optional,
/// and the default value is `/model/{name_of_model}/tf`.
///
/// `<frame_id>`: Custom `frame_id` field that this system will use as the
/// origin of the odometry transform in both the `<tf_topic>`
/// `ignition.msgs.Pose_V` message and the `<odom_topic>`
/// `ignition.msgs.Odometry` message. This element if optional, and the
/// default value is `{name_of_model}/odom`.
///
/// `<child_frame_id>`: Custom `child_frame_id` that this system will use as
/// the target of the odometry trasnform in both the `<tf_topic>`
/// `ignition.msgs.Pose_V` message and the `<odom_topic>`
/// `ignition.msgs.Odometry` message. This element if optional,
/// and the default value is `{name_of_model}/{name_of_link}`.
class IGNITION_GAZEBO_VISIBLE DiffDrive
: public System,
public ISystemConfigure,
Expand Down
184 changes: 184 additions & 0 deletions test/integration/diff_drive_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -405,6 +405,190 @@ TEST_P(DiffDriveTest, OdomCustomFrameId)
auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
node.Subscribe("/model/vehicle/odometry", odomCb);

msgs::Twist msg;
msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2));

pub.Publish(msg);

server.Run(true, 100, false);

int sleep = 0;
int maxSleep = 30;
for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ASSERT_NE(maxSleep, sleep);

EXPECT_EQ(5u, odomPosesCount);
}

/////////////////////////////////////////////////
TEST_P(DiffDriveTest, Pose_VFrameId)
{
// Start server
ServerConfig serverConfig;
serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/diff_drive.sdf");

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

server.SetUpdatePeriod(0ns);

unsigned int odomPosesCount = 0;
std::function<void(const msgs::Pose_V &)> pose_VCb =
[&odomPosesCount](const msgs::Pose_V &_msg)
{
ASSERT_TRUE(_msg.pose(0).has_header());
ASSERT_TRUE(_msg.pose(0).header().has_stamp());

ASSERT_GT(_msg.pose(0).header().data_size(), 1);

EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(),
"frame_id");
EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(),
"vehicle/odom");

EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(),
"child_frame_id");
EXPECT_STREQ(_msg.pose(0).header().data(1).value().Get(0).c_str(),
"vehicle/chassis");

odomPosesCount++;
};

transport::Node node;
auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
node.Subscribe("/model/vehicle/tf", pose_VCb);

msgs::Twist msg;
msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2));

pub.Publish(msg);

server.Run(true, 100, false);

int sleep = 0;
int maxSleep = 30;
for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ASSERT_NE(maxSleep, sleep);

EXPECT_EQ(5u, odomPosesCount);
}

/////////////////////////////////////////////////
TEST_P(DiffDriveTest, Pose_VCustomFrameId)
{
// Start server
ServerConfig serverConfig;
serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/diff_drive_custom_frame_id.sdf");

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

server.SetUpdatePeriod(0ns);

unsigned int odomPosesCount = 0;
std::function<void(const msgs::Pose_V &)> Pose_VCb =
[&odomPosesCount](const msgs::Pose_V &_msg)
{
ASSERT_TRUE(_msg.pose(0).has_header());
ASSERT_TRUE(_msg.pose(0).header().has_stamp());

ASSERT_GT(_msg.pose(0).header().data_size(), 1);

EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(),
"frame_id");
EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(),
"odom");

EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(),
"child_frame_id");
EXPECT_STREQ(_msg.pose(0).header().data(1).value().Get(0).c_str(),
"base_footprint");

odomPosesCount++;
};

transport::Node node;
auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
node.Subscribe("/model/vehicle/tf", Pose_VCb);

msgs::Twist msg;
msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2));

pub.Publish(msg);

server.Run(true, 100, false);

int sleep = 0;
int maxSleep = 30;
for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ASSERT_NE(maxSleep, sleep);

EXPECT_EQ(5u, odomPosesCount);
}

/////////////////////////////////////////////////
TEST_P(DiffDriveTest, Pose_VCustomTfTopic)
{
// Start server
ServerConfig serverConfig;
serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/diff_drive_custom_tf_topic.sdf");

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

server.SetUpdatePeriod(0ns);

unsigned int odomPosesCount = 0;
std::function<void(const msgs::Pose_V &)> pose_VCb =
[&odomPosesCount](const msgs::Pose_V &_msg)
{
ASSERT_TRUE(_msg.pose(0).has_header());
ASSERT_TRUE(_msg.pose(0).header().has_stamp());

ASSERT_GT(_msg.pose(0).header().data_size(), 1);

EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), "frame_id");
EXPECT_STREQ(
_msg.pose(0).header().data(0).value().Get(0).c_str(),
"vehicle/odom");

EXPECT_STREQ(
_msg.pose(0).header().data(1).key().c_str(), "child_frame_id");
EXPECT_STREQ(
_msg.pose(0).header().data(1).value().Get(0).c_str(),
"vehicle/chassis");

odomPosesCount++;
};

transport::Node node;
auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
node.Subscribe("/tf_foo", pose_VCb);

msgs::Twist msg;
msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2));

pub.Publish(msg);

server.Run(true, 100, false);

int sleep = 0;
Expand Down
1 change: 1 addition & 0 deletions test/integration/scene_broadcaster_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -438,6 +438,7 @@ TEST_P(SceneBroadcasterTest, State)
while (!received && sleep++ < maxSleep)
IGN_SLEEP_MS(100);
EXPECT_TRUE(received);
EXPECT_TRUE(node.Unsubscribe("/world/default/state"));

// test async state request
received = false;
Expand Down
36 changes: 13 additions & 23 deletions test/integration/velocity_control_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,43 +96,33 @@ class VelocityControlTest : public ::testing::TestWithParam<int>
// and max velocity (0.5 m/s).
// See <max_velocity< and <max_aceleration> parameters
// in "/test/worlds/velocity_control.sdf".
test::Relay velocityRamp;
const double desiredLinVel = 10.5;
const double desiredAngVel = 0.2;
velocityRamp.OnPreUpdate(
[&](const gazebo::UpdateInfo &/*_info*/,
const gazebo::EntityComponentManager &)
{
msgs::Set(msg.mutable_linear(),
math::Vector3d(desiredLinVel, 0, 0));
msgs::Set(msg.mutable_angular(),
math::Vector3d(0.0, 0, desiredAngVel));
pub.Publish(msg);
});

server.AddSystem(velocityRamp.systemPtr);
msgs::Set(msg.mutable_linear(),
math::Vector3d(desiredLinVel, 0, 0));
msgs::Set(msg.mutable_angular(),
math::Vector3d(0.0, 0, desiredAngVel));
pub.Publish(msg);

// Give some time for message to be received
std::this_thread::sleep_for(std::chrono::milliseconds(100));

server.Run(true, 3000, false);

// Poses for 4s
ASSERT_EQ(4000u, poses.size());

int sleep = 0;
int maxSleep = 30;

ASSERT_NE(maxSleep, sleep);

// verify that the vehicle is moving in +x and rotating towards +y
for (unsigned int i = 1001; i < poses.size(); ++i)
{
EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X());
EXPECT_GT(poses[i].Pos().Y(), poses[i-1].Pos().Y());
EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()) << i;
EXPECT_GT(poses[i].Pos().Y(), poses[i-1].Pos().Y()) << i;
EXPECT_NEAR(poses[i].Pos().Z(), poses[i-1].Pos().Z(), 1e-5);
EXPECT_NEAR(poses[i].Rot().Euler().X(),
poses[i-1].Rot().Euler().X(), 1e-5);
poses[i-1].Rot().Euler().X(), 1e-5) << i;
EXPECT_NEAR(poses[i].Rot().Euler().Y(),
poses[i-1].Rot().Euler().Y(), 1e-5);
EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z());
poses[i-1].Rot().Euler().Y(), 1e-5) << i;
EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i;
}
}
};
Expand Down
4 changes: 2 additions & 2 deletions test/performance/each.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,10 @@ TEST(EntityComponentManagerPerfrormance, Each)
// Initial allocation of resources can throw off calculations.
warmstart();

for (int matchingEntityCount = 1; matchingEntityCount < maxEntityCount;
for (int matchingEntityCount = 10; matchingEntityCount < maxEntityCount;
matchingEntityCount += step)
{
for (int nonmatchingEntityCount = 1;
for (int nonmatchingEntityCount = 10;
nonmatchingEntityCount < maxEntityCount;
nonmatchingEntityCount += step)
{
Expand Down
Loading

0 comments on commit 02bb83a

Please sign in to comment.