Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hi the better solution is by using an additional gazebo ros plug-in I wrote. It publishes joint states and not the tf so one can use it better with and robot_state_publisher.

# The HEADER

#ifndef V4R_ROS_TF_PUBLISHER_H
#define V4R_ROS_TF_PUBLISHER_H
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>

// ROS
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>


namespace gazebo
{
  class GazeboRosTFPublisher : public ModelPlugin
  {
    public:
      GazeboRosTFPublisher();
      ~GazeboRosTFPublisher();
      void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
      void OnUpdate(const common::UpdateInfo & _info);
      void publishTF();

    // Pointer to the model
    private:
      event::ConnectionPtr updateConnection;
      physics::WorldPtr world_;
      physics::ModelPtr parent_;
      std::vector<physics::JointPtr> joints_;

    // ROS STUFF
    boost::shared_ptr<ros::NodeHandle> rosnode_;
    boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
    sensor_msgs::JointState joint_state;
    ros::Publisher joint_state_publisher_;
    std::string tf_prefix_;
    std::string robot_namespace_;
    std::vector<std::string> joint_names_;

    // Update Rate
    double update_rate_;
    double update_period_;
    common::Time last_update_time_;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(GazeboRosTFPublisher)
}

#endif //V4R_DIFFDRIVE_PLUGIN_HH

# THE CPP

#include <v4r_gazebo_plugins/v4r_gazebo_ros_tf_publisher.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

using namespace gazebo;

GazeboRosTFPublisher::GazeboRosTFPublisher() {}

// Destructor
GazeboRosTFPublisher::~GazeboRosTFPublisher() {
    rosnode_->shutdown();
}

void GazeboRosTFPublisher::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    // Store the pointer to the model
    this->parent_ = _parent;
    this->world_ = _parent->GetWorld();

    this->robot_namespace_ = "";
    if (!_sdf->HasElement("robotNamespace")) {
        ROS_INFO("GazeboRosTFPublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
                 this->robot_namespace_.c_str());
    } else {
        this->robot_namespace_ =
                _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
    }

    if (!_sdf->HasElement("jointName")) {
        ROS_ASSERT("GazeboRosTFPublisher Plugin missing jointNames");
    } else {
        sdf::ElementPtr element = _sdf->GetElement ("jointName") ;
        std::string joint_names = element->Get<std::string>();
        boost::split(joint_names_, joint_names, boost::is_any_of(","));
    }

    this->update_rate_ = 100.0;
    if (!_sdf->HasElement("updateRate")) {
        ROS_WARN("GazeboRosTFPublisher Plugin (ns = %s) missing <updateRate>, defaults to %f",
                 this->robot_namespace_.c_str(), this->update_rate_);
    } else {
        this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
    }

    // Initialize update rate stuff
    if (this->update_rate_ > 0.0) {
        this->update_period_ = 1.0 / this->update_rate_;
    } else {
        this->update_period_ = 0.0;
    }
    last_update_time_ = this->world_->GetSimTime();

    for(unsigned int i = 0; i< joint_names_.size(); i++){
        joints_.push_back(this->parent_->GetJoint(joint_names_[i]));
        ROS_INFO("joint_name: %s", joint_names_[i].c_str() );
    }
    rosnode_ = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(this->robot_namespace_));

    ROS_INFO("Starting GazeboRosTFPublisher Plugin (ns = %s)!, %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str());

    tf_prefix_ = tf::getPrefixParam(*rosnode_);
    joint_state_publisher_ = rosnode_->advertise<sensor_msgs::JointState>("joint_states",1000);

    last_update_time_ = this->world_->GetSimTime();
    // Listen to the update event. This event is broadcast every
    // simulation iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                boost::bind(&GazeboRosTFPublisher::OnUpdate, this, _1));
}

void GazeboRosTFPublisher::OnUpdate(const common::UpdateInfo & _info)
{
    // Apply a small linear velocity to the model.
    common::Time current_time = this->world_->GetSimTime();
    double seconds_since_last_update = (current_time - last_update_time_).Double();
    if (seconds_since_last_update > update_period_) {

        publishTF();

        last_update_time_+= common::Time(update_period_);

    }

}

void GazeboRosTFPublisher::publishTF(){
    ros::Time current_time = ros::Time::now();

    joint_state.header.stamp = current_time;
    joint_state.name.resize(joints_.size());
    joint_state.position.resize(joints_.size());

    for(int i = 0; i < joints_.size(); i++){
        physics::JointPtr joint = joints_[i];
        math::Angle angle = joint->GetAngle (0);
        joint_state.name[i] = joint->GetName();
        joint_state.position[i] = angle.Radian () ;
    }
    joint_state_publisher_.publish(joint_state);
}

Hi

the better solution is by using an additional gazebo ros plug-in I wrote. It wrote, it publishes joint states and not the tf so one tf's. You can use it better with and robot_state_publisher.now the ROS robot_state_publisher to publish tf's.

The header

# The HEADER

#ifndef V4R_ROS_TF_PUBLISHER_H
JOINT_STATE_PUBLISHER_PLUGIN_HH
#define V4R_ROS_TF_PUBLISHER_H
JOINT_STATE_PUBLISHER_PLUGIN_HH

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>

// ROS
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/JointState.h>

// Usage in URDF:
//   <gazebo>
//       <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
//      <robotNamespace>/pioneer2dx</robotNamespace>
//      <jointName>chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint</jointName>
//      <updateRate>100.0</updateRate>
//      <alwaysOn>true</alwaysOn>
//       </plugin>
//   </gazebo>



namespace gazebo
{
  gazebo {
class GazeboRosTFPublisher GazeboRosJointStatePublisher : public ModelPlugin
  {
    ModelPlugin {
public:
      GazeboRosTFPublisher();
      ~GazeboRosTFPublisher();
  GazeboRosJointStatePublisher();
    ~GazeboRosJointStatePublisher();
    void Load(physics::ModelPtr Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf);
  _sdf );
    void OnUpdate(const OnUpdate ( const common::UpdateInfo & _info);
  _info );
    void publishTF();

    // Pointer to the model
 private:
     event::ConnectionPtr updateConnection;
     physics::WorldPtr world_;
     physics::ModelPtr parent_;
     std::vector<physics::JointPtr> joints_;

    // ROS STUFF
    boost::shared_ptr<ros::NodeHandle> rosnode_;
    boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
    sensor_msgs::JointState joint_state;
    ros::Publisher joint_state_publisher_;
    std::string tf_prefix_;
    std::string robot_namespace_;
    std::vector<std::string> joint_names_;

    // Update Rate
    double update_rate_;
    double update_period_;
    common::Time last_update_time_;
 };

 // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(GazeboRosTFPublisher)
GZ_REGISTER_MODEL_PLUGIN ( GazeboRosJointStatePublisher )
}

#endif //V4R_DIFFDRIVE_PLUGIN_HH
//JOINT_STATE_PUBLISHER_PLUGIN_HH

THE CPP

# THE CPP

#include <v4r_gazebo_plugins/v4r_gazebo_ros_tf_publisher.h>
<gazebo_plugins/gazebo_ros_joint_state_publisher.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

using namespace gazebo;

GazeboRosTFPublisher::GazeboRosTFPublisher() GazeboRosJointStatePublisher::GazeboRosJointStatePublisher() {}

// Destructor
GazeboRosTFPublisher::~GazeboRosTFPublisher() GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher() {
    rosnode_->shutdown();
}

void GazeboRosTFPublisher::Load(physics::ModelPtr GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf)
_sdf ) {
    // Store the pointer to the model
    this->parent_ = _parent;
    this->world_ = _parent->GetWorld();

    this->robot_namespace_ = "";
parent_->GetName ();
    if (!_sdf->HasElement("robotNamespace")) {
        ROS_INFO("GazeboRosTFPublisher ( !_sdf->HasElement ( "robotNamespace" ) ) {
        ROS_INFO ( "GazeboRosJointStatePublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
                 this->robot_namespace_.c_str());
  this->robot_namespace_.c_str() );
    } else {
        this->robot_namespace_ =
                _sdf->GetElement("robotNamespace")->Get<std::string>() + = _sdf->GetElement ( "robotNamespace" )->Get<std::string>();
        if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName ();
    }
    if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/";
    }
rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( this->robot_namespace_ ) );

    if (!_sdf->HasElement("jointName")) {
        ROS_ASSERT("GazeboRosTFPublisher ( !_sdf->HasElement ( "jointName" ) ) {
        ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames");
jointNames" );
    } else {
        sdf::ElementPtr element = _sdf->GetElement ("jointName") ( "jointName" ) ;
        std::string joint_names = element->Get<std::string>();
        boost::split(joint_names_, boost::erase_all ( joint_names, boost::is_any_of(","));
" " );
        boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) );
    }

    this->update_rate_ = 100.0;
    if (!_sdf->HasElement("updateRate")) {
        ROS_WARN("GazeboRosTFPublisher ( !_sdf->HasElement ( "updateRate" ) ) {
        ROS_WARN ( "GazeboRosJointStatePublisher Plugin (ns = %s) missing <updateRate>, defaults to %f",
                  this->robot_namespace_.c_str(), this->update_rate_);
this->update_rate_ );
    } else {
        this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
_sdf->GetElement ( "updateRate" )->Get<double>();
    }

    // Initialize update rate stuff
    if (this->update_rate_ ( this->update_rate_ > 0.0) 0.0 ) {
        this->update_period_ = 1.0 / this->update_rate_;
    } else {
        this->update_period_ = 0.0;
    }
    last_update_time_ = this->world_->GetSimTime();

    for(unsigned for ( unsigned int i = 0; i< joint_names_.size(); i++){
        joints_.push_back(this->parent_->GetJoint(joint_names_[i]));
        ROS_INFO("joint_name: i++ ) {
        joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) );
        ROS_INFO ( "joint_name: %s", joint_names_[i].c_str() );
    }
    rosnode_ = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(this->robot_namespace_));

    ROS_INFO("Starting GazeboRosTFPublisher 
    ROS_INFO ( "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str());
().c_str() );

    tf_prefix_ = tf::getPrefixParam(*rosnode_);
tf::getPrefixParam ( *rosnode_ );
    joint_state_publisher_ = rosnode_->advertise<sensor_msgs::JointState>("joint_states",1000);
rosnode_->advertise<sensor_msgs::JointState> ( "joint_states",1000 );

    last_update_time_ = this->world_->GetSimTime();
    // Listen to the update event. This event is broadcast every
    // simulation iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                boost::bind(&GazeboRosTFPublisher::OnUpdate, event::Events::ConnectWorldUpdateBegin (
                                 boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1));
_1 ) );
}

void GazeboRosTFPublisher::OnUpdate(const GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info)
_info ) {
    // Apply a small linear velocity to the model.
    common::Time current_time = this->world_->GetSimTime();
    double seconds_since_last_update = (current_time ( current_time - last_update_time_).Double();
last_update_time_ ).Double();
    if (seconds_since_last_update ( seconds_since_last_update > update_period_) update_period_ ) {

        publishTF();

        last_update_time_+= common::Time(update_period_);
common::Time ( update_period_ );

    }

}

void GazeboRosTFPublisher::publishTF(){
GazeboRosJointStatePublisher::publishTF() {
    ros::Time current_time = ros::Time::now();

    joint_state.header.stamp = current_time;
    joint_state.name.resize(joints_.size());
    joint_state.position.resize(joints_.size());

    for(int joint_state.name.resize ( joints_.size() );
    joint_state.position.resize ( joints_.size() );

    for ( int i = 0; i < joints_.size(); i++){
i++ ) {
        physics::JointPtr joint = joints_[i];
        math::Angle angle = joint->GetAngle (0);
( 0 );
        joint_state.name[i] = joint->GetName();
        joint_state.position[i] = angle.Radian () ;
    }
    joint_state_publisher_.publish(joint_state);
joint_state_publisher_.publish ( joint_state );
}