From 02cf8c7379e4c2f1128c95fa32c825c759d342c9 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 31 Jul 2019 00:54:47 +0530 Subject: [PATCH 1/4] [ros2] Port model states to ROS2 --- gazebo_ros/src/gazebo_ros_state.cpp | 63 +++++++++++++++++++ gazebo_ros/test/test_gazebo_ros_state.cpp | 40 ++++++++++++ .../test/worlds/gazebo_ros_state_test.world | 1 + gazebo_ros/worlds/gazebo_ros_state_demo.world | 7 +++ 4 files changed, 111 insertions(+) diff --git a/gazebo_ros/src/gazebo_ros_state.cpp b/gazebo_ros/src/gazebo_ros_state.cpp index 6ff0fe5e8..f793b5476 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 @@ -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::SharedPtr set_entity_state_service_; + + /// \brief ROS publisher to publish model_states. + rclcpp::Publisher::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( "set_entity_state", std::bind(&GazeboRosStatePrivate::SetEntityState, impl_.get(), std::placeholders::_1, std::placeholders::_2)); + + impl_->model_states_pub_ = impl_->ros_node_->create_publisher( + "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("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(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(model->WorldLinearVel()); + twist.angular = gazebo_ros::Convert(model->WorldAngularVel()); + model_states.twist.push_back(twist); + } + model_states_pub_->publish(model_states); + + last_update_time_ = _info.simTime; } void GazeboRosStatePrivate::GetEntityState( diff --git a/gazebo_ros/test/test_gazebo_ros_state.cpp b/gazebo_ros/test/test_gazebo_ros_state.cpp index 57ee660c3..c8dfb87ff 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 @@ -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> get_state_client_; std::shared_ptr> set_state_client_; + rclcpp::Subscription::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( + "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) diff --git a/gazebo_ros/test/worlds/gazebo_ros_state_test.world b/gazebo_ros/test/worlds/gazebo_ros_state_test.world index f06297a2f..2fa7bb772 100644 --- a/gazebo_ros/test/worlds/gazebo_ros_state_test.world +++ b/gazebo_ros/test/worlds/gazebo_ros_state_test.world @@ -4,6 +4,7 @@ /test + model_states:=model_states_test diff --git a/gazebo_ros/worlds/gazebo_ros_state_demo.world b/gazebo_ros/worlds/gazebo_ros_state_demo.world index 2005ba957..105a0860c 100644 --- a/gazebo_ros/worlds/gazebo_ros_state_demo.world +++ b/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 --> /demo + model_states:=model_states_demo + + 1.0 From 9edee5ed0f0216054503fdeb3eb85e7966786419 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 31 Jul 2019 15:26:09 +0530 Subject: [PATCH 2/4] [ros2] Port link states to ROS2 --- 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 +++ 3 files changed, 66 insertions(+) diff --git a/gazebo_ros/src/gazebo_ros_state.cpp b/gazebo_ros/src/gazebo_ros_state.cpp index f793b5476..0421dc556 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 body = boost::dynamic_pointer_cast(model->GetChild(j)); + + if (body) { + link_states.name.push_back(body->GetScopedName()); + + pose = gazebo_ros::Convert(body->WorldPose()); + link_states.pose.push_back(pose); + twist.linear = gazebo_ros::Convert(body->WorldLinearVel()); + twist.angular = gazebo_ros::Convert(body->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 From 54baf4f99f416994311fff22a7c0646d9ae4bfc8 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Thu, 8 Aug 2019 17:03:23 +0530 Subject: [PATCH 3/4] Change usage of body -> link --- gazebo_ros/src/gazebo_ros_state.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_state.cpp b/gazebo_ros/src/gazebo_ros_state.cpp index 0421dc556..f8c91cf94 100644 --- a/gazebo_ros/src/gazebo_ros_state.cpp +++ b/gazebo_ros/src/gazebo_ros_state.cpp @@ -157,15 +157,15 @@ void GazeboRosStatePrivate::OnUpdate(const gazebo::common::UpdateInfo & _info) model_states.twist.push_back(twist); for (unsigned int j = 0; j < model->GetChildCount(); ++j) { - auto body = boost::dynamic_pointer_cast(model->GetChild(j)); + auto link = boost::dynamic_pointer_cast(model->GetChild(j)); - if (body) { - link_states.name.push_back(body->GetScopedName()); + if (link) { + link_states.name.push_back(link->GetScopedName()); - pose = gazebo_ros::Convert(body->WorldPose()); + pose = gazebo_ros::Convert(link->WorldPose()); link_states.pose.push_back(pose); - twist.linear = gazebo_ros::Convert(body->WorldLinearVel()); - twist.angular = gazebo_ros::Convert(body->WorldAngularVel()); + twist.linear = gazebo_ros::Convert(link->WorldLinearVel()); + twist.angular = gazebo_ros::Convert(link->WorldAngularVel()); link_states.twist.push_back(twist); } } From 177a70f208f1f0f443a220d8cbd0d99e49256565 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Thu, 8 Aug 2019 17:09:12 +0530 Subject: [PATCH 4/4] Remove link_states from .ros1_unported --- .../gazebo_ros/gazebo_ros_api_plugin.h | 14 --- .../gazebo_ros/src/gazebo_ros_api_plugin.cpp | 92 ------------------- 2 files changed, 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 e43e8e64e..fe549d8a3 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 @@ -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 &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 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 60d169bfa..2b3c93fa2 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) { @@ -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("~/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( - "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(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_)