Merge branch 'ign-gazebo4' into particle_emitter

gazebosim · Apr 8, 2021 · 611c85a · 611c85a
2 parents f0ced9a + 8829840
commit 611c85a
Show file tree

Hide file tree

Showing 7 changed files with 336 additions and 9 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 
 #============================================================================
 # Initialize the project
@@ -136,7 +136,7 @@ ign_find_package(IgnProtobuf
                  REQUIRED
                  COMPONENTS all
                  PRETTY Protobuf)
-set(PROTOBUF_IMPORT_DIRS ${ignition-msgs6_INCLUDE_DIRS})
+set(Protobuf_IMPORT_DIRS ${ignition-msgs6_INCLUDE_DIRS})
 
 # Plugin install dirs
 set(IGNITION_GAZEBO_PLUGIN_INSTALL_DIR

diff --git a/Changelog.md b/Changelog.md
@@ -325,6 +325,115 @@
 
 ## Ignition Gazebo 3.x
 
+### Ignition Gazebo 3.X.X (202X-XX-XX)
+
+### Ignition Gazebo 3.8.0 (2021-03-17)
+
+1. Add joint position controller GUI, also enable tests for GUI plugins
+    * [Pull request #534](https://github.com/ignitionrobotics/ign-gazebo/pull/534)
+
+1. Remove visibility from headers that are not installed
+    * [Pull request #665](https://github.com/ignitionrobotics/ign-gazebo/pull/665)
+
+1. Added screenshot to toolbar
+    * [Pull request #588](https://github.com/ignitionrobotics/ign-gazebo/pull/588)
+
+1. Improve ign tool support on macOS
+    * [Pull request #477](https://github.com/ignitionrobotics/ign-gazebo/pull/477)
+
+1. change nullptr to a int ptr for qt 5.15.2 bug
+    * [Pull request #527](https://github.com/ignitionrobotics/ign-gazebo/pull/527)
+
+1. Kinetic energy monitor plugin
+    * [Pull request #492](https://github.com/ignitionrobotics/ign-gazebo/pull/492)
+
+1. Use a std::promise/std::future to avoid busy waiting the step ack messages in NetworkManagerPrimary
+    * [Pull request #470](https://github.com/ignitionrobotics/ign-gazebo/pull/470)
+
+1. clarified performer example
+    * [Pull request #390](https://github.com/ignitionrobotics/ign-gazebo/pull/390)
+
+1. Add tutorial tweaks
+    * [Pull request #380](https://github.com/ignitionrobotics/ign-gazebo/pull/380)
+
+1. Fix Qt5 warnings for using anchors
+    * [Pull request #363](https://github.com/ignitionrobotics/ign-gazebo/pull/363)
+
+1. Update codeowners
+    * [Pull request #305](https://github.com/ignitionrobotics/ign-gazebo/pull/305)
+
+1. Qt auto scale factor for HiDPI displays
+    * [Pull request #291](https://github.com/ignitionrobotics/ign-gazebo/pull/291)
+
+1. Fix yaw units
+    * [Pull request #238](https://github.com/ignitionrobotics/ign-gazebo/pull/238)
+
+1. Fixed docblock showGrid
+    * [Pull request #152](https://github.com/ignitionrobotics/ign-gazebo/pull/152)
+
+1. Fix entity tree for large worlds
+    * [Pull request #673](https://github.com/ignitionrobotics/ign-gazebo/pull/673)
+
+1. Master branch updates
+    * [Pull request #672](https://github.com/ignitionrobotics/ign-gazebo/pull/672)
+
+1. Backport #561: Use common::setenv
+    * [Pull request #666](https://github.com/ignitionrobotics/ign-gazebo/pull/666)
+
+1. Use a custom data structure to manage entity feature maps
+    * [Pull request #586](https://github.com/ignitionrobotics/ign-gazebo/pull/586)
+
+1. Limit scene broadcast publications when paused
+    * [Pull request #497](https://github.com/ignitionrobotics/ign-gazebo/pull/497)
+
+1. Fix flaky SceneBoradcaster test
+    * [Pull request #641](https://github.com/ignitionrobotics/ign-gazebo/pull/641)
+
+1. Add TF/Pose_V publisher in DiffDrive
+    * [Pull request #548](https://github.com/ignitionrobotics/ign-gazebo/pull/548)
+
+1. 👩‍🌾 Relax performance test
+    * [Pull request #640](https://github.com/ignitionrobotics/ign-gazebo/pull/640)
+
+1. 👩‍🌾 Improve velocity control test
+    * [Pull request #642](https://github.com/ignitionrobotics/ign-gazebo/pull/642)
+
+1. Add `laser_retro` support
+    * [Pull request #603](https://github.com/ignitionrobotics/ign-gazebo/pull/603)
+
+1. Fix pose of plane visual with non-default normal vector
+    * [Pull request #574](https://github.com/ignitionrobotics/ign-gazebo/pull/574)
+
+1. Add About dialog
+    * [Pull request #609](https://github.com/ignitionrobotics/ign-gazebo/pull/609)
+
+1. Make topics configurable for joint controllers
+    * [Pull request #584](https://github.com/ignitionrobotics/ign-gazebo/pull/584)
+
+1. Also use Ignition GUI render event
+    * [Pull request #598](https://github.com/ignitionrobotics/ign-gazebo/pull/598)
+
+1. Tutorial on migrating SDF files from Gazebo classic
+    * [Pull request #400](https://github.com/ignitionrobotics/ign-gazebo/pull/400)
+
+1. Visualize collisions
+    * [Pull request #531](https://github.com/ignitionrobotics/ign-gazebo/pull/531)
+
+1. Backport state update changes from pull request #486
+    * [Pull request #583](https://github.com/ignitionrobotics/ign-gazebo/pull/583)
+
+1. Publish all periodic change components in Scene Broadcaster
+    * [Pull request #544](https://github.com/ignitionrobotics/ign-gazebo/pull/544)
+
+1. added size to `ground_plane` in examples
+    * [Pull request #573](https://github.com/ignitionrobotics/ign-gazebo/pull/573)
+
+1. Parallelize State call in ECM
+    * [Pull request #451](https://github.com/ignitionrobotics/ign-gazebo/pull/451)
+
+1. Non-blocking paths request
+    * [Pull request #555](https://github.com/ignitionrobotics/ign-gazebo/pull/555)
+
 ### Ignition Gazebo 3.7.0 (2021-01-13)
 
 1. Fix examples in migration plugins tutorial.

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -10,10 +10,12 @@ set_source_files_properties(
 )
 
 # Suppress compiler warnings in generated protobuf C++ code.
-set_source_files_properties(
-  ${PROTO_PRIVATE_SRC}
-  COMPILE_FLAGS -Wno-unused-parameter
-)
+if(NOT MSVC)
+  set_source_files_properties(
+    ${PROTO_PRIVATE_SRC}
+    COMPILE_FLAGS -Wno-unused-parameter
+  )
+endif()
 
 set(network_sources
   network/NetworkConfig.cc

diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc
@@ -169,6 +169,8 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info,
   if (pose)
     msgs::Set(msg.mutable_pose(), pose->Data());
 
+  static bool hasWarned {false};
+
   // Process each joint
   for (const Entity &joint : this->joints)
   {
@@ -190,11 +192,18 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info,
       for (size_t i = 0; i < jointPositions->Data().size(); ++i)
       {
         if (i == 0)
+        {
           jointMsg->mutable_axis1()->set_position(jointPositions->Data()[i]);
+        }
         else if (i == 1)
+        {
           jointMsg->mutable_axis2()->set_position(jointPositions->Data()[i]);
-        else
+        }
+        else if (!hasWarned)
+        {
           ignwarn << "Joint state publisher only supports two joint axis\n";
+          hasWarned = true;
+        }
       }
     }
 
@@ -206,11 +215,18 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info,
       for (size_t i = 0; i < jointVelocity->Data().size(); ++i)
       {
         if (i == 0)
+        {
           jointMsg->mutable_axis1()->set_velocity(jointVelocity->Data()[i]);
+        }
         else if (i == 1)
+        {
           jointMsg->mutable_axis2()->set_velocity(jointVelocity->Data()[i]);
-        else
+        }
+        else if (!hasWarned)
+        {
           ignwarn << "Joint state publisher only supports two joint axis\n";
+          hasWarned = true;
+        }
       }
     }
 
@@ -222,11 +238,18 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info,
       for (size_t i = 0; i < jointForce->Data().size(); ++i)
       {
         if (i == 0)
+        {
           jointMsg->mutable_axis1()->set_force(jointForce->Data()[i]);
+        }
         else if (i == 1)
+        {
           jointMsg->mutable_axis2()->set_force(jointForce->Data()[i]);
-        else
+        }
+        else if (!hasWarned)
+        {
           ignwarn << "Joint state publisher only supports two joint axis\n";
+          hasWarned = true;
+        }
       }
     }
   }

diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc
@@ -38,6 +38,7 @@
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/Static.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/World.hh"
@@ -124,6 +125,15 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate
   public: static void AddVisuals(msgs::Link *_msg, const Entity _entity,
                                  const SceneGraphType &_graph);
 
+  /// \brief Adds sensors to a msgs::Link object based on the contents of
+  /// the scene graph
+  /// \param[inout] _msg Pointer to msg object to which the sensors will be
+  /// added.
+  /// \param[in] _entity Parent entity in the graph
+  /// \param[in] _graph Scene graph
+  public: static void AddSensors(msgs::Link *_msg, const Entity _entity,
+                                 const SceneGraphType &_graph);
+
   /// \brief Recursively remove entities from the graph
   /// \param[in] _entity Entity
   /// \param[in/out] _graph Scene graph
@@ -742,6 +752,26 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities(
         return true;
       });
 
+  // Sensors
+  _manager.EachNew<components::Sensor, components::Name,
+                   components::ParentEntity, components::Pose>(
+      [&](const Entity &_entity, const components::Sensor *,
+          const components::Name *_nameComp,
+          const components::ParentEntity *_parentComp,
+          const components::Pose *_poseComp) -> bool
+      {
+        auto sensorMsg = std::make_shared<msgs::Sensor>();
+        sensorMsg->set_id(_entity);
+        sensorMsg->set_parent_id(_parentComp->Data());
+        sensorMsg->set_name(_nameComp->Data());
+        sensorMsg->mutable_pose()->CopyFrom(msgs::Convert(_poseComp->Data()));
+
+        // Add to graph
+        newGraph.AddVertex(_nameComp->Data(), sensorMsg, _entity);
+        newGraph.AddEdge({_parentComp->Data(), _entity}, true);
+        newEntity = true;
+        return true;
+      });
 
   // Update the whole scene graph from the new graph
   {
@@ -886,6 +916,24 @@ void SceneBroadcasterPrivate::AddVisuals(msgs::Link *_msg, const Entity _entity,
   }
 }
 
+//////////////////////////////////////////////////
+void SceneBroadcasterPrivate::AddSensors(msgs::Link *_msg, const Entity _entity,
+    const SceneGraphType &_graph)
+{
+  if (!_msg)
+    return;
+
+  for (const auto &vertex : _graph.AdjacentsFrom(_entity))
+  {
+    auto sensorMsg = std::dynamic_pointer_cast<msgs::Sensor>(
+        vertex.second.get().Data());
+    if (!sensorMsg)
+      continue;
+
+    _msg->add_sensor()->CopyFrom(*sensorMsg);
+  }
+}
+
 //////////////////////////////////////////////////
 void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity,
                                        const SceneGraphType &_graph)
@@ -908,6 +956,9 @@ void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity,
 
     // Lights
     AddLights(msgOut, vertex.second.get().Id(), _graph);
+
+    // Sensors
+    AddSensors(msgOut, vertex.second.get().Id(), _graph);
   }
 }
 

diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc
@@ -220,6 +220,57 @@ TEST_P(SceneBroadcasterTest, SceneTopic)
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene));
 }
 
+/////////////////////////////////////////////////
+/// Test whether the scene topic is published only when new entities are added
+TEST_P(SceneBroadcasterTest, SceneTopicSensors)
+{
+  // Start server
+  ignition::gazebo::ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+                          "/test/worlds/altimeter_with_pose.sdf");
+
+  gazebo::Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+  EXPECT_EQ(12u, *server.EntityCount());
+
+  // Create requester
+  transport::Node node;
+
+  std::vector<msgs::Scene> sceneMsgs;
+  std::function<void(const msgs::Scene &)> collectMsgs =
+      [&sceneMsgs](const msgs::Scene &_msg)
+      {
+        sceneMsgs.push_back(_msg);
+      };
+
+  node.Subscribe("/world/altimeter_sensor/scene/info", collectMsgs);
+
+  // Run server
+  server.Run(true, 10, false);
+
+  // Should only have one scene even though the simulation ran multiple times
+  ASSERT_EQ(1u, sceneMsgs.size());
+
+  // Compare this scene with one from a service request
+  msgs::Scene &scene = sceneMsgs.front();
+
+  bool result{false};
+  unsigned int timeout{5000};
+  ignition::msgs::Scene msg;
+
+  EXPECT_TRUE(node.Request("/world/altimeter_sensor/scene/info",
+        timeout, msg, result));
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene));
+
+  EXPECT_EQ(1, msg.model(1).link(0).sensor_size());
+  EXPECT_EQ("altimeter_sensor", msg.model(1).link(0).sensor(0).name());
+  EXPECT_DOUBLE_EQ(0.1, msg.model(1).link(0).sensor(0).pose().position().x());
+  EXPECT_DOUBLE_EQ(0.2, msg.model(1).link(0).sensor(0).pose().position().y());
+  EXPECT_DOUBLE_EQ(0.3, msg.model(1).link(0).sensor(0).pose().position().z());
+}
+
 /////////////////////////////////////////////////
 /// Test whether the scene topic is published only when new entities are added
 TEST_P(SceneBroadcasterTest, DeletedTopic)