Skip to content

Commit

Permalink
[ros2] Port Link states to ROS2 (ros-simulation#969)
Browse files Browse the repository at this point in the history 
* [ros2] Port model states to ROS2

* [ros2] Port link states to ROS2

* Change usage of body -> link

* Remove link_states from .ros1_unported
  • Loading branch information
@shiveshkhaitan
shiveshkhaitan committed Aug 15, 2019
1 parent 0a6d4a7 commit 75931ba
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 106 deletions.
14 changes: 0 additions & 14 deletions .ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -157,9 +149,6 @@ class GazeboRosApiPlugin : public SystemPlugin
void publishSimTime(const boost::shared_ptr<gazebo::msgs::WorldStatistics const> &msg);
void publishSimTime();

/// \brief
void publishLinkStates();

/// \brief Used for the dynamic reconfigure callback function template
void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level);

Expand Down Expand Up @@ -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_;
Expand All @@ -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<ros::AsyncSpinner> async_ros_spin_;
Expand Down
92 changes: 0 additions & 92 deletions .ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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");
Expand Down Expand Up @@ -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<gazebo::msgs::Light>("~/light/modify");

// reset topic connection counts
pub_link_states_connection_count_ = 0;

/// \brief advertise all services
if (enable_ros_network_)
advertiseServices();
Expand Down Expand Up @@ -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<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 =
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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<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_)
Expand Down
26 changes: 26 additions & 0 deletions gazebo_ros/src/gazebo_ros_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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_;

Expand Down Expand Up @@ -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));
Expand All @@ -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()) {
Expand All @@ -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;
}
Expand Down
35 changes: 35 additions & 0 deletions gazebo_ros/test/test_gazebo_ros_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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_;
};

Expand Down Expand Up @@ -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)
Expand Down
5 changes: 5 additions & 0 deletions gazebo_ros/worlds/gazebo_ros_state_demo.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down

0 comments on commit 75931ba

Please sign in to comment.