Revision history [back]

An alternative to writing
a Gazebo Plugin that publishes force/torque data to a ROS Topic is to write
a ROS Node that listens to the Gazebo Transport layer and republishes force/torque data to a ROS Topic.

Example:

my_robot.urdf

...
  <gazebo reference="joint_name">
    <sensor name="joint_name_force_torque" type="force_torque">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <visualize>true</visualize>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
...

gazebo_transport_to_ros_topic.cpp

/*
*  Name: gazebo_transport_to_ros_topic.cpp
*  Author: Joseph Coombe
*  Date: 11/22/2017
*  Edited: 11/27/2017
*  Description:
*   Subscribe to a Gazebo transport topic and publish to a ROS topic
*/

// Gazebo dependencies
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>

// ROS dependencies
#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>

#include <iostream>

ros::Publisher pub;

void torquesCb(const ConstWrenchStampedPtr &_msg)
{
  std::cout << "Received msg: " << std::endl;
  std::cout << _msg->DebugString() << std::endl;
  geometry_msgs::WrenchStamped msgWrenchedStamped;
  // try WrenchStamped msgWrenchedStamped;
  msgWrenchedStamped.header.stamp = ros::Time::now();
  msgWrenchedStamped.wrench.force.x = _msg->wrench().force().x();
  msgWrenchedStamped.wrench.force.y = _msg->wrench().force().y();
  msgWrenchedStamped.wrench.force.z = _msg->wrench().force().z();
  msgWrenchedStamped.wrench.torque.x = _msg->wrench().torque().x();
  msgWrenchedStamped.wrench.torque.y = _msg->wrench().torque().y();
  msgWrenchedStamped.wrench.torque.z = _msg->wrench().torque().z();
  pub.publish(msgWrenchedStamped);
}

int main(int argc, char **argv)
{
  ROS_INFO("Starting gazebo_transport_to_ros_topic node");

  // Load Gazebo
  gazebo::client::setup(argc, argv);

  // Load ROS
  ros::init(argc, argv, "gazebo_transport_to_ros_topic");

  // Create Gazebo node and init
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Create ROS node and init
  ros::NodeHandle n;

  // Get ROS params
  std::string gazebo_transport_topic_to_sub;
  std::string ros_topic_to_pub;
  double ros_rate;
  ros::param::get("~gazebo_transport_topic_to_sub", gazebo_transport_topic_to_sub);
  ros::param::get("~ros_rate", ros_rate);

  pub = n.advertise<geometry_msgs::WrenchStamped>(ros_topic_to_pub, 100);

  // Listen to Gazebo force_torque sensor topic
  gazebo::transport::SubscriberPtr sub = node->Subscribe(gazebo_transport_topic_to_sub, torquesCb);
  ros::Rate loop_rate(ros_rate); // 100 hz
  while(ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  gazebo::shutdown();
  return 0;
}

my_robot.launch

...
  <node name="joint_name" pkg="my_pkg_gazebo" type="gazebo_transport_to_ros_topic"
        output="screen" launch-prefix="xterm -e" >
    <param name="gazebo_transport_topic_to_sub" type="str" value="/gazebo/default/robot_name/joint_name/joint_name_force_torque/wrench" />
    <param name="ros_topic_to_pub" type="str" value="force_torque/robot_name/joint_name_force_torque" />
    <param name="ros_rate" type="double" value="100.0" />
  </node>
...

@sloretz's answer is probably better. I did it this way - in my case - because I wanted to access some options that were not available in the gazebo_ros force/torque plugin.

Example:

my_robot.urdf

...
  <gazebo reference="joint_name">
    <sensor name="joint_name_force_torque" type="force_torque">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <visualize>true</visualize>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
...

gazebo_transport_to_ros_topic.cpp

/*
*  Name: gazebo_transport_to_ros_topic.cpp
*  Author: Joseph Coombe
*  Date: 11/22/2017
*  Edited: 11/27/2017
*  Description:
*   Subscribe to a Gazebo transport topic and publish to a ROS topic
*/

// Gazebo dependencies
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>

// ROS dependencies
#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>

#include <iostream>

ros::Publisher pub;

void torquesCb(const ConstWrenchStampedPtr &_msg)
{
  std::cout << "Received msg: " << std::endl;
  std::cout << _msg->DebugString() << std::endl;
  geometry_msgs::WrenchStamped msgWrenchedStamped;
  // try WrenchStamped msgWrenchedStamped;
  msgWrenchedStamped.header.stamp = ros::Time::now();
  msgWrenchedStamped.wrench.force.x = _msg->wrench().force().x();
  msgWrenchedStamped.wrench.force.y = _msg->wrench().force().y();
  msgWrenchedStamped.wrench.force.z = _msg->wrench().force().z();
  msgWrenchedStamped.wrench.torque.x = _msg->wrench().torque().x();
  msgWrenchedStamped.wrench.torque.y = _msg->wrench().torque().y();
  msgWrenchedStamped.wrench.torque.z = _msg->wrench().torque().z();
  pub.publish(msgWrenchedStamped);
}

int main(int argc, char **argv)
{
  ROS_INFO("Starting gazebo_transport_to_ros_topic node");

  // Load Gazebo
  gazebo::client::setup(argc, argv);

  // Load ROS
  ros::init(argc, argv, "gazebo_transport_to_ros_topic");

  // Create Gazebo node and init
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Create ROS node and init
  ros::NodeHandle n;

  // Get ROS params
  std::string gazebo_transport_topic_to_sub;
  std::string ros_topic_to_pub;
  double ros_rate;
  ros::param::get("~gazebo_transport_topic_to_sub", gazebo_transport_topic_to_sub);
  ros::param::get("~ros_rate", ros_rate);

  pub = n.advertise<geometry_msgs::WrenchStamped>(ros_topic_to_pub, 100);

  // Listen to Gazebo force_torque sensor topic
  gazebo::transport::SubscriberPtr sub = node->Subscribe(gazebo_transport_topic_to_sub, torquesCb);
  ros::Rate loop_rate(ros_rate); // 100 hz
  while(ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  gazebo::shutdown();
  return 0;
}

my_robot.launch

...
  <node name="joint_name" pkg="my_pkg_gazebo" type="gazebo_transport_to_ros_topic"
        output="screen" launch-prefix="xterm -e" >
    <param name="gazebo_transport_topic_to_sub" type="str" value="/gazebo/default/robot_name/joint_name/joint_name_force_torque/wrench" />
    <param name="ros_topic_to_pub" type="str" value="force_torque/robot_name/joint_name_force_torque" />
    <param name="ros_rate" type="double" value="100.0" />
  </node>
...

Uses the Gazebo Force/Torque Sensor.

@sloretz's answer is probably better. I did it this way - in my case - because I wanted to access some options that were not available in the gazebo_ros force/torque plugin.

Example:

my_robot.urdf

...
  <gazebo reference="joint_name">
    <sensor name="joint_name_force_torque" type="force_torque">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <visualize>true</visualize>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
...

gazebo_transport_to_ros_topic.cpp

/*
*  Name: gazebo_transport_to_ros_topic.cpp
*  Author: Joseph Coombe
*  Date: 11/22/2017
*  Edited: 11/27/2017
*  Description:
*   Subscribe to a Gazebo transport topic and publish to a ROS topic
*/

// Gazebo dependencies
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>

// ROS dependencies
#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>

#include <iostream>

ros::Publisher pub;

void torquesCb(const ConstWrenchStampedPtr &_msg)
{
  std::cout << "Received msg: " << std::endl;
  std::cout << _msg->DebugString() << std::endl;
  geometry_msgs::WrenchStamped msgWrenchedStamped;
  // try WrenchStamped msgWrenchedStamped;
  msgWrenchedStamped.header.stamp = ros::Time::now();
  msgWrenchedStamped.wrench.force.x = _msg->wrench().force().x();
  msgWrenchedStamped.wrench.force.y = _msg->wrench().force().y();
  msgWrenchedStamped.wrench.force.z = _msg->wrench().force().z();
  msgWrenchedStamped.wrench.torque.x = _msg->wrench().torque().x();
  msgWrenchedStamped.wrench.torque.y = _msg->wrench().torque().y();
  msgWrenchedStamped.wrench.torque.z = _msg->wrench().torque().z();
  pub.publish(msgWrenchedStamped);
}

int main(int argc, char **argv)
{
  ROS_INFO("Starting gazebo_transport_to_ros_topic node");

  // Load Gazebo
  gazebo::client::setup(argc, argv);

  // Load ROS
  ros::init(argc, argv, "gazebo_transport_to_ros_topic");

  // Create Gazebo node and init
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Create ROS node and init
  ros::NodeHandle n;

  // Get ROS params
  std::string gazebo_transport_topic_to_sub;
  std::string ros_topic_to_pub;
  double ros_rate;
  ros::param::get("~gazebo_transport_topic_to_sub", gazebo_transport_topic_to_sub);
  ros::param::get("~ros_rate", ros_rate);

  pub = n.advertise<geometry_msgs::WrenchStamped>(ros_topic_to_pub, 100);

  // Listen to Gazebo force_torque sensor topic
  gazebo::transport::SubscriberPtr sub = node->Subscribe(gazebo_transport_topic_to_sub, torquesCb);
  ros::Rate loop_rate(ros_rate); // 100 hz
  while(ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  gazebo::shutdown();
  return 0;
}

my_robot.launch

...
  <node name="joint_name" pkg="my_pkg_gazebo" type="gazebo_transport_to_ros_topic"
        output="screen" launch-prefix="xterm -e" >
    <param name="gazebo_transport_topic_to_sub" type="str" value="/gazebo/default/robot_name/joint_name/joint_name_force_torque/wrench" />
    <param name="ros_topic_to_pub" type="str" value="force_torque/robot_name/joint_name_force_torque" />
    <param name="ros_rate" type="double" value="100.0" />
  </node>
...

Uses the Gazebo Force/Torque Sensor.

@sloretz's answer is probably better. I did it this way - in my case - because I wanted to access some options that were not available in the gazebo_ros force/torque plugin.

Example:

my_robot.urdf

...
  <gazebo reference="joint_name">
    <sensor name="joint_name_force_torque" type="force_torque">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <visualize>true</visualize>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
...

~~gazebo_transport_to_ros_topic.cpp~~gazebo_transport_to_ros_topic.cpp (place in src directory of ROS package)

/*
*  Name: gazebo_transport_to_ros_topic.cpp
*  Author: Joseph Coombe
*  Date: 11/22/2017
*  Edited: 11/27/2017
*  Description:
*   Subscribe to a Gazebo transport topic and publish to a ROS topic
*/

// Gazebo dependencies
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>

// ROS dependencies
#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>

#include <iostream>

ros::Publisher pub;

void torquesCb(const ConstWrenchStampedPtr &_msg)
{
  std::cout << "Received msg: " << std::endl;
  std::cout << _msg->DebugString() << std::endl;
  geometry_msgs::WrenchStamped msgWrenchedStamped;
  // try WrenchStamped msgWrenchedStamped;
  msgWrenchedStamped.header.stamp = ros::Time::now();
  msgWrenchedStamped.wrench.force.x = _msg->wrench().force().x();
  msgWrenchedStamped.wrench.force.y = _msg->wrench().force().y();
  msgWrenchedStamped.wrench.force.z = _msg->wrench().force().z();
  msgWrenchedStamped.wrench.torque.x = _msg->wrench().torque().x();
  msgWrenchedStamped.wrench.torque.y = _msg->wrench().torque().y();
  msgWrenchedStamped.wrench.torque.z = _msg->wrench().torque().z();
  pub.publish(msgWrenchedStamped);
}

int main(int argc, char **argv)
{
  ROS_INFO("Starting gazebo_transport_to_ros_topic node");

  // Load Gazebo
  gazebo::client::setup(argc, argv);

  // Load ROS
  ros::init(argc, argv, "gazebo_transport_to_ros_topic");

  // Create Gazebo node and init
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Create ROS node and init
  ros::NodeHandle n;

  // Get ROS params
  std::string gazebo_transport_topic_to_sub;
  std::string ros_topic_to_pub;
  double ros_rate;
  ros::param::get("~gazebo_transport_topic_to_sub", gazebo_transport_topic_to_sub);
  ros::param::get("~ros_rate", ros_rate);

  pub = n.advertise<geometry_msgs::WrenchStamped>(ros_topic_to_pub, 100);

  // Listen to Gazebo force_torque sensor topic
  gazebo::transport::SubscriberPtr sub = node->Subscribe(gazebo_transport_topic_to_sub, torquesCb);
  ros::Rate loop_rate(ros_rate); // 100 hz
  while(ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  gazebo::shutdown();
  return 0;
}

Make sure to add the following sections to your CMakeLists.txt (sorry this is a bit messy):

...
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
# Find Gazebo
find_package(gazebo REQUIRED)

set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")

link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(include)
include_directories( ${catkin_INCLUDE_DIRS}
                     ${Boost_INCLUDE_DIR}
                     ${GAZEBO_INCLUDE_DIRS}
)

target_link_libraries(
  ${PROJECT_NAME}_gazebo_transport_to_ros_topic
  ${catkin_LIBRARIES}
  ${GAZEBO_LIBRARIES}
)
...

my_robot.launch

...
  <node name="joint_name" pkg="my_pkg_gazebo" type="gazebo_transport_to_ros_topic"
        output="screen" launch-prefix="xterm -e" >
    <param name="gazebo_transport_topic_to_sub" type="str" value="/gazebo/default/robot_name/joint_name/joint_name_force_torque/wrench" />
    <param name="ros_topic_to_pub" type="str" value="force_torque/robot_name/joint_name_force_torque" />
    <param name="ros_rate" type="double" value="100.0" />
  </node>
...