[ros2] Port Link states to ROS2

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

@@ -69,8 +69,6 @@
 #include "gazebo_msgs/SetLightProperties.h"
 
 // Topics
-#include "gazebo_msgs/LinkStates.h"
-
 #include "geometry_msgs/Vector3.h"
 #include "geometry_msgs/Wrench.h"
 #include "geometry_msgs/Pose.h"
@@ -119,12 +117,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \brief advertise services
   void advertiseServices();
 
-  /// \brief
-  void onLinkStatesConnect();
-
-  /// \brief
-  void onLinkStatesDisconnect();
-
   /// \brief
   bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req,gazebo_msgs::GetModelProperties::Response &res);
 
@@ -184,9 +176,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   void publishSimTime(const boost::shared_ptr<gazebo::msgs::WorldStatistics const> &msg);
   void publishSimTime();
 
-  /// \brief
-  void publishLinkStates();
-
   /// \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,
@@ -224,7 +213,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   gazebo::event::ConnectionPtr wrench_update_event_;
   gazebo::event::ConnectionPtr force_update_event_;
   gazebo::event::ConnectionPtr time_update_event_;
-  gazebo::event::ConnectionPtr pub_link_states_event_;
   gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_;
 
   ros::ServiceServer get_model_properties_service_;
@@ -242,8 +230,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   ros::ServiceServer set_model_configuration_service_;
   ros::ServiceServer clear_joint_forces_service_;
   ros::ServiceServer clear_body_wrenches_service_;
-  ros::Publisher     pub_link_states_;
-  int                pub_link_states_connection_count_;
 
   // ROS comm
   boost::shared_ptr<ros::AsyncSpinner> async_ros_spin_;

.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp

@@ -32,7 +32,6 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() :
   world_created_(false),
   stop_(false),
   plugin_loaded_(false),
-  pub_link_states_connection_count_(0),
   pub_clock_frequency_(0),
   enable_ros_network_(true)
 {
@@ -61,10 +60,6 @@ GazeboRosApiPlugin::~GazeboRosApiPlugin()
   time_update_event_.reset();
   ROS_DEBUG_STREAM_NAMED("api_plugin","Slots disconnected");
 
-  if (pub_link_states_connection_count_ > 0) // disconnect if there are subscribers on exit
-    pub_link_states_event_.reset();
-  ROS_DEBUG_STREAM_NAMED("api_plugin","Disconnected World Updates");
-
   // Stop the multi threaded ROS spinner
   async_ros_spin_->stop();
   ROS_DEBUG_STREAM_NAMED("api_plugin","Async ROS Spin Stopped");
@@ -184,9 +179,6 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
   //stat_sub_ = gazebonode_->Subscribe("~/world_stats", &GazeboRosApiPlugin::publishSimTime, this); // TODO: does not work in server plugin?
   light_modify_pub_ = gazebonode_->Advertise<gazebo::msgs::Light>("~/light/modify");
 
-  // reset topic connection counts
-  pub_link_states_connection_count_ = 0;
-
   /// \brief advertise all services
   if (enable_ros_network_)
     advertiseServices();
@@ -292,15 +284,6 @@ void GazeboRosApiPlugin::advertiseServices()
                                                                             ros::VoidPtr(), &gazebo_queue_);
   get_physics_properties_service_ = nh_->advertiseService(get_physics_properties_aso);
 
-  // publish complete link states in world frame
-  ros::AdvertiseOptions pub_link_states_ao =
-    ros::AdvertiseOptions::create<gazebo_msgs::LinkStates>(
-                                                           "link_states",10,
-                                                           boost::bind(&GazeboRosApiPlugin::onLinkStatesConnect,this),
-                                                           boost::bind(&GazeboRosApiPlugin::onLinkStatesDisconnect,this),
-                                                           ros::VoidPtr(), &gazebo_queue_);
-  pub_link_states_ = nh_->advertise(pub_link_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 =
@@ -386,24 +369,6 @@ void GazeboRosApiPlugin::advertiseServices()
 #endif
 }
 
-void GazeboRosApiPlugin::onLinkStatesConnect()
-{
-  pub_link_states_connection_count_++;
-  if (pub_link_states_connection_count_ == 1) // connect on first subscriber
-    pub_link_states_event_   = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishLinkStates,this));
-}
-
-void GazeboRosApiPlugin::onLinkStatesDisconnect()
-{
-  pub_link_states_connection_count_--;
-  if (pub_link_states_connection_count_ <= 0) // disconnect with no subscribers
-  {
-    pub_link_states_event_.reset();
-    if (pub_link_states_connection_count_ < 0) // should not be possible
-      ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
-  }
-}
-
 bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req,
                                             gazebo_msgs::GetModelProperties::Response &res)
 {
@@ -1254,63 +1219,6 @@ void GazeboRosApiPlugin::publishSimTime()
   pub_clock_.publish(ros_time_);
 }
 
-void GazeboRosApiPlugin::publishLinkStates()
-{
-  gazebo_msgs::LinkStates link_states;
-
-  // fill link_states
-#if GAZEBO_MAJOR_VERSION >= 8
-  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
-  {
-    gazebo::physics::ModelPtr model = world_->ModelByIndex(i);
-#else
-  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
-  {
-    gazebo::physics::ModelPtr model = world_->GetModel(i);
-#endif
-
-    for (unsigned int j = 0 ; j < model->GetChildCount(); j ++)
-    {
-      gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(j));
-
-      if (body)
-      {
-        link_states.name.push_back(body->GetScopedName());
-        geometry_msgs::Pose pose;
-#if GAZEBO_MAJOR_VERSION >= 8
-        ignition::math::Pose3d  body_pose = body->WorldPose(); // - myBody->GetCoMPose();
-        ignition::math::Vector3d linear_vel  = body->WorldLinearVel();
-        ignition::math::Vector3d angular_vel = body->WorldAngularVel();
-#else
-        ignition::math::Pose3d  body_pose = body->GetWorldPose().Ign(); // - myBody->GetCoMPose();
-        ignition::math::Vector3d linear_vel  = body->GetWorldLinearVel().Ign();
-        ignition::math::Vector3d angular_vel = body->GetWorldAngularVel().Ign();
-#endif
-        ignition::math::Vector3d pos = body_pose.Pos();
-        ignition::math::Quaterniond rot = body_pose.Rot();
-        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();
-        link_states.pose.push_back(pose);
-        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();
-        link_states.twist.push_back(twist);
-      }
-    }
-  }
-
-  pub_link_states_.publish(link_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/link_states.hpp>
 #include <gazebo_msgs/msg/model_states.hpp>
 #include <gazebo_msgs/srv/get_entity_state.hpp>
 #include <gazebo_msgs/srv/set_entity_state.hpp>
@@ -68,6 +69,9 @@ class GazeboRosStatePrivate
   /// \brief ROS publisher to publish model_states.
   rclcpp::Publisher<gazebo_msgs::msg::ModelStates>::SharedPtr model_states_pub_;
 
+  /// \brief ROS publisher to publish link_states.
+  rclcpp::Publisher<gazebo_msgs::msg::LinkStates>::SharedPtr link_states_pub_;
+
   /// \brief Connection to world update event, called at every iteration
   gazebo::event::ConnectionPtr world_update_event_;
 
@@ -109,6 +113,12 @@ void GazeboRosState::Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf
   RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing states of gazebo models at [%s]",
     impl_->model_states_pub_->get_topic_name());
 
+  impl_->link_states_pub_ = impl_->ros_node_->create_publisher<gazebo_msgs::msg::LinkStates>(
+    "link_states", rclcpp::QoS(rclcpp::KeepLast(1)));
+
+  RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing states of gazebo links at [%s]",
+    impl_->link_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));
@@ -132,6 +142,7 @@ void GazeboRosStatePrivate::OnUpdate(const gazebo::common::UpdateInfo & _info)
   }
 
   gazebo_msgs::msg::ModelStates model_states;
+  gazebo_msgs::msg::LinkStates link_states;
 
   // fill model_states
   for (const auto & model : world_->Models()) {
@@ -144,8 +155,23 @@ void GazeboRosStatePrivate::OnUpdate(const gazebo::common::UpdateInfo & _info)
     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);
+
+    for (unsigned int j = 0; j < model->GetChildCount(); ++j) {
+      auto link = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(j));
+
+      if (link) {
+        link_states.name.push_back(link->GetScopedName());
+
+        pose = gazebo_ros::Convert<geometry_msgs::msg::Pose>(link->WorldPose());
+        link_states.pose.push_back(pose);
+        twist.linear = gazebo_ros::Convert<geometry_msgs::msg::Vector3>(link->WorldLinearVel());
+        twist.angular = gazebo_ros::Convert<geometry_msgs::msg::Vector3>(link->WorldAngularVel());
+        link_states.twist.push_back(twist);
+      }
+    }
   }
   model_states_pub_->publish(model_states);
+  link_states_pub_->publish(link_states);
 
   last_update_time_ = _info.simTime;
 }

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/link_states.hpp>
 #include <gazebo_msgs/msg/model_states.hpp>
 #include <gazebo_msgs/srv/get_entity_state.hpp>
 #include <gazebo_msgs/srv/set_entity_state.hpp>
@@ -53,6 +54,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::LinkStates>::SharedPtr link_states_sub_;
   rclcpp::Subscription<gazebo_msgs::msg::ModelStates>::SharedPtr model_states_sub_;
 };
 
@@ -219,6 +221,39 @@ TEST_F(GazeboRosStateTest, GetSet)
     EXPECT_NEAR(model_states_msg->pose[1].orientation.z, 0.0, tol);
     EXPECT_NEAR(model_states_msg->pose[1].orientation.w, 1.0, tol);
   }
+
+  // Link states
+  {
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node_);
+    gazebo_msgs::msg::LinkStates::SharedPtr link_states_msg{nullptr};
+    link_states_sub_ = node_->create_subscription<gazebo_msgs::msg::LinkStates>(
+      "test/link_states_test", rclcpp::SystemDefaultsQoS(),
+      [&link_states_msg](gazebo_msgs::msg::LinkStates::SharedPtr _msg) {
+        link_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 && !link_states_msg; ++sleep) {
+      executor.spin_once(100ms);
+      gazebo::common::Time::MSleep(100);
+    }
+    EXPECT_NE(sleep, maxSleep);
+    EXPECT_NE(link_states_msg, nullptr);
+    EXPECT_NEAR(link_states_msg->pose[1].position.x, 1.0, tol);
+    EXPECT_NEAR(link_states_msg->pose[1].position.y, 2.0, tol);
+    EXPECT_NEAR(link_states_msg->pose[1].position.z, 0.5, tol);
+    EXPECT_NEAR(link_states_msg->pose[1].orientation.x, 0.0, tol);
+    EXPECT_NEAR(link_states_msg->pose[1].orientation.y, 0.0, tol);
+    EXPECT_NEAR(link_states_msg->pose[1].orientation.z, 0.0, tol);
+    EXPECT_NEAR(link_states_msg->pose[1].orientation.w, 1.0, tol);
+  }
 }
 
 int main(int argc, char ** argv)

gazebo_ros/worlds/gazebo_ros_state_demo.world

@@ -31,13 +31,18 @@
   Try listening to model states:
 
       ros2 topic echo /demo/model_states_demo
+
+  Try listening to link states:
+
+      ros2 topic echo /demo/link_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>
+        <argument>link_states:=link_states_demo</argument>
       </ros>
 
       <update_rate>1.0</update_rate>