[ros2] Port model states to ROS2

.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h

@@ -69,7 +69,6 @@
 #include "gazebo_msgs/SetLightProperties.h"
 
 // Topics
-#include "gazebo_msgs/ModelStates.h"
 #include "gazebo_msgs/LinkStates.h"
 
 #include "geometry_msgs/Vector3.h"
@@ -123,15 +122,9 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \brief
   void onLinkStatesConnect();
 
-  /// \brief
-  void onModelStatesConnect();
-
   /// \brief
   void onLinkStatesDisconnect();
 
-  /// \brief
-  void onModelStatesDisconnect();
-
   /// \brief
   bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req,gazebo_msgs::GetModelProperties::Response &res);
 
@@ -194,9 +187,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \brief
   void publishLinkStates();
 
-  /// \brief
-  void publishModelStates();
-
   /// \brief helper function for applyBodyWrench
   ///        shift wrench from reference frame to target frame
   void transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque,
@@ -235,7 +225,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   gazebo::event::ConnectionPtr force_update_event_;
   gazebo::event::ConnectionPtr time_update_event_;
   gazebo::event::ConnectionPtr pub_link_states_event_;
-  gazebo::event::ConnectionPtr pub_model_states_event_;
   gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_;
 
   ros::ServiceServer get_model_properties_service_;
@@ -254,9 +243,7 @@ class GazeboRosApiPlugin : public SystemPlugin
   ros::ServiceServer clear_joint_forces_service_;
   ros::ServiceServer clear_body_wrenches_service_;
   ros::Publisher     pub_link_states_;
-  ros::Publisher     pub_model_states_;
   int                pub_link_states_connection_count_;
-  int                pub_model_states_connection_count_;
 
   // ROS comm
   boost::shared_ptr<ros::AsyncSpinner> async_ros_spin_;

.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp

@@ -33,7 +33,6 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() :
   stop_(false),
   plugin_loaded_(false),
   pub_link_states_connection_count_(0),
-  pub_model_states_connection_count_(0),
   pub_clock_frequency_(0),
   enable_ros_network_(true)
 {
@@ -64,8 +63,6 @@ GazeboRosApiPlugin::~GazeboRosApiPlugin()
 
   if (pub_link_states_connection_count_ > 0) // disconnect if there are subscribers on exit
     pub_link_states_event_.reset();
-  if (pub_model_states_connection_count_ > 0) // disconnect if there are subscribers on exit
-    pub_model_states_event_.reset();
   ROS_DEBUG_STREAM_NAMED("api_plugin","Disconnected World Updates");
 
   // Stop the multi threaded ROS spinner
@@ -189,7 +186,6 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
 
   // reset topic connection counts
   pub_link_states_connection_count_ = 0;
-  pub_model_states_connection_count_ = 0;
 
   /// \brief advertise all services
   if (enable_ros_network_)
@@ -305,15 +301,6 @@ void GazeboRosApiPlugin::advertiseServices()
                                                            ros::VoidPtr(), &gazebo_queue_);
   pub_link_states_ = nh_->advertise(pub_link_states_ao);
 
-  // publish complete model states in world frame
-  ros::AdvertiseOptions pub_model_states_ao =
-    ros::AdvertiseOptions::create<gazebo_msgs::ModelStates>(
-                                                            "model_states",10,
-                                                            boost::bind(&GazeboRosApiPlugin::onModelStatesConnect,this),
-                                                            boost::bind(&GazeboRosApiPlugin::onModelStatesDisconnect,this),
-                                                            ros::VoidPtr(), &gazebo_queue_);
-  pub_model_states_ = nh_->advertise(pub_model_states_ao);
-
   // Advertise more services on the custom queue
   std::string set_link_properties_service_name("set_link_properties");
   ros::AdvertiseServiceOptions set_link_properties_aso =
@@ -406,13 +393,6 @@ void GazeboRosApiPlugin::onLinkStatesConnect()
     pub_link_states_event_   = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishLinkStates,this));
 }
 
-void GazeboRosApiPlugin::onModelStatesConnect()
-{
-  pub_model_states_connection_count_++;
-  if (pub_model_states_connection_count_ == 1) // connect on first subscriber
-    pub_model_states_event_   = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishModelStates,this));
-}
-
 void GazeboRosApiPlugin::onLinkStatesDisconnect()
 {
   pub_link_states_connection_count_--;
@@ -424,17 +404,6 @@ void GazeboRosApiPlugin::onLinkStatesDisconnect()
   }
 }
 
-void GazeboRosApiPlugin::onModelStatesDisconnect()
-{
-  pub_model_states_connection_count_--;
-  if (pub_model_states_connection_count_ <= 0) // disconnect with no subscribers
-  {
-    pub_model_states_event_.reset();
-    if (pub_model_states_connection_count_ < 0) // should not be possible
-      ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
-  }
-}
-
 bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req,
                                             gazebo_msgs::GetModelProperties::Response &res)
 {
@@ -1342,50 +1311,6 @@ void GazeboRosApiPlugin::publishLinkStates()
   pub_link_states_.publish(link_states);
 }
 
-void GazeboRosApiPlugin::publishModelStates()
-{
-  gazebo_msgs::ModelStates model_states;
-
-  // fill model_states
-#if GAZEBO_MAJOR_VERSION >= 8
-  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
-  {
-    gazebo::physics::ModelPtr model = world_->ModelByIndex(i);
-    ignition::math::Pose3d  model_pose = model->WorldPose(); // - myBody->GetCoMPose();
-    ignition::math::Vector3d linear_vel  = model->WorldLinearVel();
-    ignition::math::Vector3d angular_vel = model->WorldAngularVel();
-#else
-  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
-  {
-    gazebo::physics::ModelPtr model = world_->GetModel(i);
-    ignition::math::Pose3d  model_pose = model->GetWorldPose().Ign(); // - myBody->GetCoMPose();
-    ignition::math::Vector3d linear_vel  = model->GetWorldLinearVel().Ign();
-    ignition::math::Vector3d angular_vel = model->GetWorldAngularVel().Ign();
-#endif
-    ignition::math::Vector3d pos = model_pose.Pos();
-    ignition::math::Quaterniond rot = model_pose.Rot();
-    geometry_msgs::Pose pose;
-    pose.position.x = pos.X();
-    pose.position.y = pos.Y();
-    pose.position.z = pos.Z();
-    pose.orientation.w = rot.W();
-    pose.orientation.x = rot.X();
-    pose.orientation.y = rot.Y();
-    pose.orientation.z = rot.Z();
-    model_states.pose.push_back(pose);
-    model_states.name.push_back(model->GetName());
-    geometry_msgs::Twist twist;
-    twist.linear.x = linear_vel.X();
-    twist.linear.y = linear_vel.Y();
-    twist.linear.z = linear_vel.Z();
-    twist.angular.x = angular_vel.X();
-    twist.angular.y = angular_vel.Y();
-    twist.angular.z = angular_vel.Z();
-    model_states.twist.push_back(twist);
-  }
-  pub_model_states_.publish(model_states);
-}
-
 void GazeboRosApiPlugin::physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level)
 {
   if (!physics_reconfigure_initialized_)

gazebo_ros/src/gazebo_ros_state.cpp

@@ -18,6 +18,7 @@
 #include <gazebo/physics/Link.hh>
 #include <gazebo/physics/Model.hh>
 #include <gazebo/physics/World.hh>
+#include <gazebo_msgs/msg/model_states.hpp>
 #include <gazebo_msgs/srv/get_entity_state.hpp>
 #include <gazebo_msgs/srv/set_entity_state.hpp>
 #include <gazebo_ros/node.hpp>
@@ -34,6 +35,10 @@ namespace gazebo_ros
 class GazeboRosStatePrivate
 {
 public:
+  /// Callback when world is updated.
+  /// \param[in] _info Updated simulation info.
+  void OnUpdate(const gazebo::common::UpdateInfo & _info);
+
   /// \brief Callback for get entity state service.
   /// \param[in] req Request
   /// \param[out] res Response
@@ -59,6 +64,18 @@ class GazeboRosStatePrivate
 
   /// \brief ROS service to handle requests to set entity states.
   rclcpp::Service<gazebo_msgs::srv::SetEntityState>::SharedPtr set_entity_state_service_;
+
+  /// \brief ROS publisher to publish model_states.
+  rclcpp::Publisher<gazebo_msgs::msg::ModelStates>::SharedPtr model_states_pub_;
+
+  /// \brief Connection to world update event, called at every iteration
+  gazebo::event::ConnectionPtr world_update_event_;
+
+  /// \brief Update period in seconds.
+  double update_period_;
+
+  /// \brief Last update time.
+  gazebo::common::Time last_update_time_;
 };
 
 GazeboRosState::GazeboRosState()
@@ -85,6 +102,52 @@ void GazeboRosState::Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf
     impl_->ros_node_->create_service<gazebo_msgs::srv::SetEntityState>(
     "set_entity_state", std::bind(&GazeboRosStatePrivate::SetEntityState, impl_.get(),
     std::placeholders::_1, std::placeholders::_2));
+
+  impl_->model_states_pub_ = impl_->ros_node_->create_publisher<gazebo_msgs::msg::ModelStates>(
+    "model_states", rclcpp::QoS(rclcpp::KeepLast(1)));
+
+  RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing states of gazebo models at [%s]",
+    impl_->model_states_pub_->get_topic_name());
+
+  // Get a callback when world is updated
+  impl_->world_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(
+    std::bind(&GazeboRosStatePrivate::OnUpdate, impl_.get(), std::placeholders::_1));
+
+  // Update rate
+  auto update_rate = _sdf->Get<double>("update_rate", 100.0).first;
+  if (update_rate > 0.0) {
+    impl_->update_period_ = 1.0 / update_rate;
+  } else {
+    impl_->update_period_ = 0.0;
+  }
+  impl_->last_update_time_ = _world->SimTime();
+}
+
+void GazeboRosStatePrivate::OnUpdate(const gazebo::common::UpdateInfo & _info)
+{
+  double seconds_since_last_update = (_info.simTime - last_update_time_).Double();
+
+  if (seconds_since_last_update < update_period_) {
+    return;
+  }
+
+  gazebo_msgs::msg::ModelStates model_states;
+
+  // fill model_states
+  for (const auto & model : world_->Models()) {
+    auto pose = gazebo_ros::Convert<geometry_msgs::msg::Pose>(model->WorldPose());
+
+    model_states.pose.push_back(pose);
+    model_states.name.push_back(model->GetName());
+
+    geometry_msgs::msg::Twist twist;
+    twist.linear = gazebo_ros::Convert<geometry_msgs::msg::Vector3>(model->WorldLinearVel());
+    twist.angular = gazebo_ros::Convert<geometry_msgs::msg::Vector3>(model->WorldAngularVel());
+    model_states.twist.push_back(twist);
+  }
+  model_states_pub_->publish(model_states);
+
+  last_update_time_ = _info.simTime;
 }
 
 void GazeboRosStatePrivate::GetEntityState(

gazebo_ros/test/test_gazebo_ros_state.cpp

@@ -16,6 +16,7 @@
 #include <ignition/math/Pose3.hh>
 #include <ignition/math/Vector3.hh>
 
+#include <gazebo_msgs/msg/model_states.hpp>
 #include <gazebo_msgs/srv/get_entity_state.hpp>
 #include <gazebo_msgs/srv/set_entity_state.hpp>
 #include <gazebo_ros/conversions/geometry_msgs.hpp>
@@ -26,6 +27,8 @@
 
 #define tol 10e-2
 
+using namespace std::literals::chrono_literals; // NOLINT
+
 class GazeboRosStateTest : public gazebo::ServerFixture
 {
 public:
@@ -50,6 +53,7 @@ class GazeboRosStateTest : public gazebo::ServerFixture
   rclcpp::Node::SharedPtr node_;
   std::shared_ptr<rclcpp::Client<gazebo_msgs::srv::GetEntityState>> get_state_client_;
   std::shared_ptr<rclcpp::Client<gazebo_msgs::srv::SetEntityState>> set_state_client_;
+  rclcpp::Subscription<gazebo_msgs::msg::ModelStates>::SharedPtr model_states_sub_;
 };
 
 void GazeboRosStateTest::SetUp()
@@ -179,6 +183,42 @@ TEST_F(GazeboRosStateTest, GetSet)
     this->GetState("boxes::top", ignition::math::Pose3d(10, 20, 30, 0.1, 0, 0),
       ignition::math::Vector3d(1.0, 2.0, 3.0), ignition::math::Vector3d(0.0, 0.0, 4.0));
   }
+
+  // Model states
+  {
+    // Set new state
+    this->SetState("boxes", ignition::math::Pose3d(1, 2, 0.5, 0, 0, 0));
+
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node_);
+    gazebo_msgs::msg::ModelStates::SharedPtr model_states_msg{nullptr};
+    model_states_sub_ = node_->create_subscription<gazebo_msgs::msg::ModelStates>(
+      "test/model_states_test", rclcpp::SystemDefaultsQoS(),
+      [&model_states_msg](gazebo_msgs::msg::ModelStates::SharedPtr _msg) {
+        model_states_msg = _msg;
+      });
+
+    // Wait for a message
+    world_->Step(1000);
+
+    // Wait for it to be processed
+    int sleep{0};
+    int maxSleep{300};
+
+    for (; sleep < maxSleep && !model_states_msg; ++sleep) {
+      executor.spin_once(100ms);
+      gazebo::common::Time::MSleep(100);
+    }
+    EXPECT_NE(sleep, maxSleep);
+    EXPECT_NE(model_states_msg, nullptr);
+    EXPECT_NEAR(model_states_msg->pose[1].position.x, 1.0, tol);
+    EXPECT_NEAR(model_states_msg->pose[1].position.y, 2.0, tol);
+    EXPECT_NEAR(model_states_msg->pose[1].position.z, 0.5, tol);
+    EXPECT_NEAR(model_states_msg->pose[1].orientation.x, 0.0, tol);
+    EXPECT_NEAR(model_states_msg->pose[1].orientation.y, 0.0, tol);
+    EXPECT_NEAR(model_states_msg->pose[1].orientation.z, 0.0, tol);
+    EXPECT_NEAR(model_states_msg->pose[1].orientation.w, 1.0, tol);
+  }
 }
 
 int main(int argc, char ** argv)

gazebo_ros/test/worlds/gazebo_ros_state_test.world

@@ -4,6 +4,7 @@
     <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
       <ros>
         <namespace>/test</namespace>
+        <argument>model_states:=model_states_test</argument>
       </ros>
     </plugin>
 

gazebo_ros/worlds/gazebo_ros_state_demo.world

@@ -27,13 +27,20 @@
   Try teleporting a model:
 
       ros2 service call /demo/set_entity_state 'gazebo_msgs/SetEntityState' '{state: {name: "boxes::top", reference_frame: "boxes::top", twist: {angular: {x: 2}}}}'
+
+  Try listening to model states:
+
+      ros2 topic echo /demo/model_states_demo
 -->
 <sdf version="1.6">
   <world name="default">
     <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
       <ros>
         <namespace>/demo</namespace>
+        <argument>model_states:=model_states_demo</argument>
       </ros>
+
+      <update_rate>1.0</update_rate>
     </plugin>
 
     <include>