Merge branch 'ign-gazebo6' into aditya/odom_pub_noise

gazebosim · Mar 25, 2022 · 8aa4abb · 8aa4abb
2 parents bef7837 + 9b07b24
commit 8aa4abb
Show file tree

Hide file tree

Showing 6 changed files with 314 additions and 89 deletions.
diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt
@@ -15,7 +15,7 @@ libignition-tools-dev
 libignition-transport11-dev
 libignition-utils1-cli-dev
 libogre-1.9-dev
-libogre-2.1-dev
+libogre-2.2-dev
 libprotobuf-dev
 libprotoc-dev
 libsdformat12-dev

diff --git a/docker/Dockerfile.base b/docker/Dockerfile.base
@@ -11,10 +11,6 @@ RUN scripts/enable_ign_stable.sh
 COPY docker/scripts/install_common_deps.sh scripts/install_common_deps.sh
 RUN scripts/install_common_deps.sh
 
-# This is not strictly necessary, but makes things faster down the line.
-COPY docker/scripts/install_ign_deps.sh scripts/install_ign_deps.sh
-RUN scripts/install_ign_deps.sh
-
 COPY docker/scripts/enable_gcc8.sh scripts/enable_gcc8.sh
 RUN scripts/enable_gcc8.sh
 
diff --git a/docker/scripts/install_ign_deps.sh b/docker/scripts/install_ign_deps.sh
diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc
@@ -161,8 +161,10 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info,
       {
         topics.push_back(this->topic);
       }
-      topics.push_back(std::string("/world/") + worldName + "/model/"
-        + this->model.Name(_ecm) + "/joint_state");
+      std::string topicStr =
+          topicFromScopedName(this->model.Entity(), _ecm, false) +
+          "/joint_state";
+      topics.push_back(topicStr);
 
       this->topic = validTopic(topics);
       if (this->topic.empty())

diff --git a/test/integration/joint_state_publisher_system.cc b/test/integration/joint_state_publisher_system.cc
@@ -144,3 +144,57 @@ TEST_F(JointStatePublisherTest,
   // Make sure the callback was triggered at least once.
   EXPECT_GT(count, 0);
 }
+
+/////////////////////////////////////////////////
+TEST_F(JointStatePublisherTest,
+       IGN_UTILS_TEST_DISABLED_ON_WIN32(NestedJointPublisher))
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "test", "worlds", "diff_drive_nested.sdf"));
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  server.SetUpdatePeriod(0ns);
+
+  int count = 0;
+  // Check that all of joints are published.
+  std::function<void(const msgs::Model &)> jointStateCb =
+    [&](const msgs::Model &_msg)
+    {
+      bool foundLeftWheelJoint{false},
+           foundRightWheelJoint{false},
+           foundCasterWheel{false},
+           extra{false};
+
+      for (int i = 0; i < _msg.joint_size(); ++i)
+      {
+        if (_msg.joint(i).name() == "left_wheel_joint")
+          foundLeftWheelJoint = true;
+        else if (_msg.joint(i).name() == "right_wheel_joint")
+          foundRightWheelJoint = true;
+        else if (_msg.joint(i).name() == "caster_wheel")
+          foundCasterWheel = true;
+        else
+          extra = true;
+      }
+      EXPECT_TRUE(foundLeftWheelJoint);
+      EXPECT_TRUE(foundRightWheelJoint);
+      EXPECT_TRUE(foundCasterWheel);
+      EXPECT_FALSE(extra);
+      count++;
+    };
+
+  transport::Node node;
+  node.Subscribe(
+      "/world/diff_drive_nested/model/vehicle/model/vehicle_nested/joint_state",
+      jointStateCb);
+
+  server.Run(true, 10, false);
+
+  // Make sure the callback was triggered at least once.
+  EXPECT_GT(count, 0);
+}