Skip to content

Commit 8741f23

Browse files
jacobperronjacobperron
committed
Make p3d offset element names singular (#1210)
* Make p3d offset element names singular - <xyz_offsets> is renamed to <xyz_offset> - <rpy_offsets> is renamed to <rpy_offsets> The old names can still be used, but are deprecated. This is more consistent with the naming convention used in ROS 1 versions. Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Add test for deprecated functionality Signed-off-by: Jacob Perron <jacob@openrobotics.org>
1 parent 5ab46cf commit 8741f23

File tree

5 files changed

+111
-11
lines changed

5 files changed

+111
-11
lines changed

‎gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -52,10 +52,10 @@ class GazeboRosP3DPrivate;
5252
<update_rate>1</update_rate>
5353
5454
<!-- Translation offset to be added to the pose. -->
55-
<xyz_offsets>10 10 10</xyz_offsets>
55+
<xyz_offset>10 10 10</xyz_offset>
5656
5757
<!-- Rotation offset to be added to the pose, in Euler angles. -->
58-
<rpy_offsets>0.1 0.1 0.1</rpy_offsets>
58+
<rpy_offset>0.1 0.1 0.1</rpy_offset>
5959
6060
<!-- Standard deviation of the noise to be added to the reported velocities. -->
6161
<gaussian_noise>0.01</gaussian_noise>

‎gazebo_plugins/src/gazebo_ros_p3d.cpp

+21-6
Original file line numberDiff line numberDiff line change
@@ -130,18 +130,33 @@ void GazeboRosP3D::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
130130
RCLCPP_DEBUG(
131131
impl_->ros_node_->get_logger(), "Publishing on topic [%s]", impl_->topic_name_.c_str());
132132

133-
if (!sdf->HasElement("xyz_offsets")) {
134-
RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <xyz_offsets>, defaults to 0s");
135-
} else {
133+
if (sdf->HasElement("xyz_offsets")) {
134+
RCLCPP_WARN(
135+
impl_->ros_node_->get_logger(), "<xyz_offsets> is deprecated, use <xyz_offset> instead.");
136136
impl_->offset_.Pos() = sdf->GetElement("xyz_offsets")->Get<ignition::math::Vector3d>();
137137
}
138-
139-
if (!sdf->HasElement("rpy_offsets")) {
140-
RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <rpy_offsets>, defaults to 0s");
138+
if (!sdf->HasElement("xyz_offset")) {
139+
if (!sdf->HasElement("xyz_offsets")) {
140+
RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <xyz_offset>, defaults to 0s");
141+
}
141142
} else {
143+
impl_->offset_.Pos() = sdf->GetElement("xyz_offset")->Get<ignition::math::Vector3d>();
144+
}
145+
146+
if (sdf->HasElement("rpy_offsets")) {
147+
RCLCPP_WARN(
148+
impl_->ros_node_->get_logger(), "<rpy_offsets> is deprecated, use <rpy_offset> instead.");
142149
impl_->offset_.Rot() = ignition::math::Quaterniond(
143150
sdf->GetElement("rpy_offsets")->Get<ignition::math::Vector3d>());
144151
}
152+
if (!sdf->HasElement("rpy_offset")) {
153+
if (!sdf->HasElement("rpy_offsets")) {
154+
RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <rpy_offset>, defaults to 0s");
155+
}
156+
} else {
157+
impl_->offset_.Rot() = ignition::math::Quaterniond(
158+
sdf->GetElement("rpy_offset")->Get<ignition::math::Vector3d>());
159+
}
145160

146161
if (!sdf->HasElement("gaussian_noise")) {
147162
RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <gassian_noise>, defaults to 0.0");

‎gazebo_plugins/test/test_gazebo_ros_p3d.cpp

+73
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,79 @@ TEST_F(GazeboRosP3dTest, Publishing)
100100
EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.z, tol);
101101
}
102102

103+
TEST_F(GazeboRosP3dTest, DeprecatedOffsetElements)
104+
{
105+
// Load test world and start paused
106+
this->Load("worlds/gazebo_ros_p3d.world", true);
107+
108+
// World
109+
auto world = gazebo::physics::get_world();
110+
ASSERT_NE(nullptr, world);
111+
112+
// Model
113+
auto model = world->ModelByName("the_model");
114+
ASSERT_NE(nullptr, model);
115+
116+
// Link
117+
auto link = model->GetLink("box_link");
118+
ASSERT_NE(nullptr, link);
119+
120+
// Reference link
121+
auto ref_link = model->GetLink("sphere_link");
122+
ASSERT_NE(nullptr, ref_link);
123+
124+
// Step a bit for model to settle
125+
world->Step(100);
126+
127+
// Create node and executor
128+
auto node = std::make_shared<rclcpp::Node>("gazebo_ros_p3d_test");
129+
ASSERT_NE(nullptr, node);
130+
131+
rclcpp::executors::SingleThreadedExecutor executor;
132+
executor.add_node(node);
133+
134+
// Create subscriber
135+
nav_msgs::msg::Odometry::SharedPtr latest_msg{nullptr};
136+
auto sub = node->create_subscription<nav_msgs::msg::Odometry>(
137+
"test_deprecated/p3d_test_deprecated", rclcpp::SensorDataQoS(),
138+
[&latest_msg](const nav_msgs::msg::Odometry::SharedPtr _msg) {
139+
latest_msg = _msg;
140+
});
141+
142+
// Wait for a message
143+
world->Step(1000);
144+
unsigned int max_sleep{30u};
145+
for (auto sleep = 0u; !latest_msg && sleep < max_sleep; ++sleep) {
146+
executor.spin_once(100ms);
147+
gazebo::common::Time::MSleep(100);
148+
}
149+
150+
// Check message
151+
ASSERT_NE(nullptr, latest_msg);
152+
EXPECT_EQ("sphere_link", latest_msg->header.frame_id);
153+
EXPECT_EQ("box_link", latest_msg->child_frame_id);
154+
155+
EXPECT_NEAR(
156+
latest_msg->pose.pose.position.x,
157+
-ref_link->WorldPose().Pos().X() + link->WorldPose().Pos().X() + 5, tol);
158+
EXPECT_NEAR(
159+
latest_msg->pose.pose.position.y,
160+
-ref_link->WorldPose().Pos().Y() + link->WorldPose().Pos().Y() + 5, tol);
161+
EXPECT_NEAR(
162+
latest_msg->pose.pose.position.z,
163+
-ref_link->WorldPose().Pos().Z() + link->WorldPose().Pos().Z() + 5, tol);
164+
EXPECT_NEAR(latest_msg->pose.pose.orientation.x, link->WorldPose().Rot().X(), tol);
165+
EXPECT_NEAR(latest_msg->pose.pose.orientation.y, link->WorldPose().Rot().Y(), tol);
166+
EXPECT_NEAR(latest_msg->pose.pose.orientation.z, link->WorldPose().Rot().Z(), tol);
167+
168+
EXPECT_NEAR(0.0, latest_msg->twist.twist.linear.x, tol);
169+
EXPECT_NEAR(0.0, latest_msg->twist.twist.linear.y, tol);
170+
EXPECT_NEAR(0.0, latest_msg->twist.twist.linear.z, tol);
171+
EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.x, tol);
172+
EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.y, tol);
173+
EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.z, tol);
174+
}
175+
103176
int main(int argc, char ** argv)
104177
{
105178
rclcpp::init(argc, argv);

‎gazebo_plugins/test/worlds/gazebo_ros_p3d.world

+13-1
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,19 @@
5050
<body_name>box_link</body_name>
5151
<frame_name>sphere_link</frame_name>
5252
<update_rate>0</update_rate>
53-
<xyz_offsets>10 10 10</xyz_offsets>
53+
<xyz_offset>10 10 10</xyz_offset>
54+
<rpy_offset>0 0 0</rpy_offset>
55+
<gaussian_noise>0</gaussian_noise>
56+
</plugin>
57+
<plugin name="gazebo_ros_p3d_deprecated" filename="libgazebo_ros_p3d.so">
58+
<ros>
59+
<namespace>/test_deprecated</namespace>
60+
<remapping>odom:=p3d_test_deprecated</remapping>
61+
</ros>
62+
<body_name>box_link</body_name>
63+
<frame_name>sphere_link</frame_name>
64+
<update_rate>0</update_rate>
65+
<xyz_offsets>5 5 5</xyz_offsets>
5466
<rpy_offsets>0 0 0</rpy_offsets>
5567
<gaussian_noise>0</gaussian_noise>
5668
</plugin>

‎gazebo_plugins/worlds/gazebo_ros_p3d_demo.world

+2-2
Original file line numberDiff line numberDiff line change
@@ -57,8 +57,8 @@
5757
<body_name>box_link</body_name>
5858
<frame_name>sphere_link</frame_name>
5959
<update_rate>1</update_rate>
60-
<xyz_offsets>10 10 10</xyz_offsets>
61-
<rpy_offsets>0.1 0.1 0.1</rpy_offsets>
60+
<xyz_offset>10 10 10</xyz_offset>
61+
<rpy_offset>0.1 0.1 0.1</rpy_offset>
6262
<gaussian_noise>0.01</gaussian_noise>
6363
</plugin>
6464
</model>

0 commit comments

Comments
 (0)