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
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
Comments