Robotics StackExchange | Archived questions

ROS differential drive controller does not publish TF's for wheels

Hi

ubuntu: 12.04 64biz gazebo: 2.0.0 ros: hydro

I have a modelled pioneer3DX (not the one from the server) and I am using a URDF file described by xarco to generate a robot description for ROS and Gazebo but it looks like that gazebo/or the ros plugin is not publishing the tf's for joints with a continuous joint type.
The problem is now that rviz shows the wheels on a incorrect pose and I have incorrect tf.
My question is now who should publish this tf's the differentialdrivecontroller plug-in? I would love to up-load an image and the workspace but my karma is to low :-)

Greetings Markus

Asked by Markus Bader on 2013-10-30 03:03:25 UTC

Comments

Answers

Hi

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

The header

#ifndef JOINT_STATE_PUBLISHER_PLUGIN_HH
#define 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 {
class GazeboRosJointStatePublisher : public ModelPlugin {
public:
    GazeboRosJointStatePublisher();
    ~GazeboRosJointStatePublisher();
    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_;
    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 ( GazeboRosJointStatePublisher )
}

#endif //JOINT_STATE_PUBLISHER_PLUGIN_HH

THE CPP

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

using namespace gazebo;

GazeboRosJointStatePublisher::GazeboRosJointStatePublisher() {}

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

void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _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 ( "GazeboRosJointStatePublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
                   this->robot_namespace_.c_str() );
    } else {
        this->robot_namespace_ = _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 ( "GazeboRosJointStatePublisher Plugin missing jointNames" );
    } else {
        sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ;
        std::string joint_names = element->Get<std::string>();
        boost::erase_all ( joint_names, " " );
        boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) );
    }

    this->update_rate_ = 100.0;
    if ( !_sdf->HasElement ( "updateRate" ) ) {
        ROS_WARN ( "GazeboRosJointStatePublisher 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() );
    }

    ROS_INFO ( "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %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 ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) );
}

void GazeboRosJointStatePublisher::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 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 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 );
}

Asked by Markus Bader on 2013-10-30 11:38:20 UTC

Comments

Nice. I still see it as a valuable contribution to ros_gazebo_pkgs, please consider to send it upstream. By the way, could you please mark this answer as correct with the check button? Thanks.

Asked by Jose Luis Rivero on 2013-10-31 06:32:12 UTC

I cannot mark this answer as correct. The system tells me I need at least 20 points to mark my question as answered :-(

Asked by Markus Bader on 2013-11-04 03:25:21 UTC

I placed a pull request on the ros_gazebo_pkgs, hopfully soon there will be the source available. I made also changes on the differential_drive_controller plug-in to publish TF/Joint States based on an xml option.

Asked by Markus Bader on 2013-11-05 05:26:13 UTC

You rock! Thanks very much. https://github.com/ros-simulation/gazebo_ros_pkgs/pull/141

Asked by Jose Luis Rivero on 2013-11-05 20:03:54 UTC

Why not just make them static if they do not move except in rotation to the base.

Asked by rnunziata on 2013-11-04 18:09:43 UTC

Comments

Yes I could but if a fixed joint type in gazebo cannot be actuated and on the other hand I like to see my wheels spinning :-)

Asked by Markus Bader on 2013-11-05 03:32:27 UTC

I am just saying not for gazebo but only for the tf. The wheels will spin in gazebo.

Asked by rnunziata on 2013-11-05 15:19:07 UTC

@munziata: can you please tell how to fix the tf of wheels with respect to robot body but keep it a continuous joint.

Asked by keshav_sarraf on 2014-06-24 07:40:45 UTC