From 75931bad564c23fe77c590600125d474599398db Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Sun, 11 Aug 2019 10:25:52 +0530 Subject: [PATCH] [ros2] Port Link states to ROS2 (#969) * [ros2] Port model states to ROS2 * [ros2] Port link states to ROS2 * Change usage of body -> link * Remove link_states from .ros1_unported --- .../gazebo_ros/gazebo_ros_api_plugin.h | 14 --- .../gazebo_ros/src/gazebo_ros_api_plugin.cpp | 92 ------------------- gazebo_ros/src/gazebo_ros_state.cpp | 26 ++++++ gazebo_ros/test/test_gazebo_ros_state.cpp | 35 +++++++ gazebo_ros/worlds/gazebo_ros_state_demo.world | 5 + 5 files changed, 66 insertions(+), 106 deletions(-) diff --git a/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h b/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h index 76b5539ed..f1cd2fa61 100644 --- a/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h +++ b/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h @@ -63,8 +63,6 @@ #include "gazebo_msgs/SetLightProperties.h" // Topics -#include "gazebo_msgs/LinkStates.h" - #include "geometry_msgs/Vector3.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/Twist.h" @@ -112,12 +110,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); @@ -157,9 +149,6 @@ class GazeboRosApiPlugin : public SystemPlugin void publishSimTime(const boost::shared_ptr &msg); void publishSimTime(); - /// \brief - void publishLinkStates(); - /// \brief Used for the dynamic reconfigure callback function template void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level); @@ -188,7 +177,6 @@ class GazeboRosApiPlugin : public SystemPlugin gazebo::physics::WorldPtr world_; 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_; @@ -202,8 +190,6 @@ class GazeboRosApiPlugin : public SystemPlugin ros::ServiceServer get_physics_properties_service_; ros::ServiceServer set_joint_properties_service_; ros::ServiceServer set_model_configuration_service_; - ros::Publisher pub_link_states_; - int pub_link_states_connection_count_; // ROS comm boost::shared_ptr async_ros_spin_; diff --git a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 9396f1bff..385232b7c 100644 --- a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/.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) { @@ -59,10 +58,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"); @@ -164,9 +159,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("~/light/modify"); - // reset topic connection counts - pub_link_states_connection_count_ = 0; - /// \brief advertise all services if (enable_ros_network_) advertiseServices(); @@ -270,15 +262,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( - "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 = @@ -328,24 +311,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) { @@ -876,63 +841,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(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_) diff --git a/gazebo_ros/src/gazebo_ros_state.cpp b/gazebo_ros/src/gazebo_ros_state.cpp index f793b5476..f8c91cf94 100644 --- a/gazebo_ros/src/gazebo_ros_state.cpp +++ b/gazebo_ros/src/gazebo_ros_state.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,9 @@ class GazeboRosStatePrivate /// \brief ROS publisher to publish model_states. rclcpp::Publisher::SharedPtr model_states_pub_; + /// \brief ROS publisher to publish link_states. + rclcpp::Publisher::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( + "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(model->WorldLinearVel()); twist.angular = gazebo_ros::Convert(model->WorldAngularVel()); model_states.twist.push_back(twist); + + for (unsigned int j = 0; j < model->GetChildCount(); ++j) { + auto link = boost::dynamic_pointer_cast(model->GetChild(j)); + + if (link) { + link_states.name.push_back(link->GetScopedName()); + + pose = gazebo_ros::Convert(link->WorldPose()); + link_states.pose.push_back(pose); + twist.linear = gazebo_ros::Convert(link->WorldLinearVel()); + twist.angular = gazebo_ros::Convert(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; } diff --git a/gazebo_ros/test/test_gazebo_ros_state.cpp b/gazebo_ros/test/test_gazebo_ros_state.cpp index c8dfb87ff..1ddf23c37 100644 --- a/gazebo_ros/test/test_gazebo_ros_state.cpp +++ b/gazebo_ros/test/test_gazebo_ros_state.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,7 @@ class GazeboRosStateTest : public gazebo::ServerFixture rclcpp::Node::SharedPtr node_; std::shared_ptr> get_state_client_; std::shared_ptr> set_state_client_; + rclcpp::Subscription::SharedPtr link_states_sub_; rclcpp::Subscription::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( + "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) diff --git a/gazebo_ros/worlds/gazebo_ros_state_demo.world b/gazebo_ros/worlds/gazebo_ros_state_demo.world index 105a0860c..1aaab2f7e 100644 --- a/gazebo_ros/worlds/gazebo_ros_state_demo.world +++ b/gazebo_ros/worlds/gazebo_ros_state_demo.world @@ -31,6 +31,10 @@ Try listening to model states: ros2 topic echo /demo/model_states_demo + + Try listening to link states: + + ros2 topic echo /demo/link_states_demo --> @@ -38,6 +42,7 @@ /demo model_states:=model_states_demo + link_states:=link_states_demo 1.0