system plugin to publish /clock from gazebo 1.7.1 standalone to ROS

edit

Hi,

I have not installed simulator_gazebo in ros-groovy and currently working with gazebo standalone 1.7.1. I need to publish /clock as in simulator_gazebo. I figured that it was done by the gazebo_ros_api_plugin. However, gazebo_ros_api_plugin do much more than publishing /clock and I isolated the following parts for my purpose. But, still it seems something is missing as /clock is not advertised. Any help would be appreciated.

Also, how do you run the gzclient with this plugin? I tried with gzclient -g libmy_gazebo_ros_clock.so and it didnt work.

Thank you

#include <my_gazebo_plugins gazebo_ros_clock.h="">

namespace gazebo
{


SystemClock::SystemClock()
{
    this->world_created_ = false;
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
SystemClock::~SystemClock()
{
    event::Events::DisconnectWorldUpdateBegin(this->time_update_event_);
    // Finalize the controller
    this->rosnode_->shutdown();
    delete this->rosnode_;

    // shutdown ros queue
    this->gazebo_callback_queue_thread_->join();
    delete this->gazebo_callback_queue_thread_;
}

/// \brief ros queue thread for this node
void SystemClock::gazeboQueueThread()
{
    ROS_DEBUG_STREAM("Callback thread id=" << boost::this_thread::get_id());
    static const double timeout = 0.001;
    while (this->rosnode_->ok())
    this->gazebo_queue_.callAvailable(ros::WallDuration(timeout));
}

void SystemClock::Load(int argc, char** argv)
{
    // setup ros related
    if (!ros::isInitialized())
        ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler);
    else
        ROS_ERROR("Something other than this gazebo_ros_api plugin started ros::init(...), command line arguments may not be parsed properly.");

    this->rosnode_ = new ros::NodeHandle("~");

    /// \brief setup custom callback queue
    gazebo_callback_queue_thread_ = new boost::thread(&SystemClock::gazeboQueueThread,this);

    // below needs the world to be created first
    this->load_gazebo_ros_clock_plugin_event_ = event::Events::ConnectWorldCreated(boost::bind(&SystemClock::LoadGazeboClockPlugin,this,_1));
}

void SystemClock::LoadGazeboClockPlugin(std::string _worldName)
{
    // make sure things are only called once
    event::Events::DisconnectWorldCreated(this->load_gazebo_ros_clock_plugin_event_);
    this->lock_.lock();
    if (this->world_created_)
    {
        this->lock_.unlock();
        return;
    }

    // set flag to true and load this plugin
    this->world_created_ = true;
    this->lock_.unlock();


    this->world = physics::get_world(_worldName);
    if (!this->world)
    {
        ROS_FATAL("cannot load gazebo ros api server plugin, physics::get_world() fails to return world");
        return;
    }

    this->gazebonode_ = transport::NodePtr(new transport::Node());
    this->gazebonode_->Init(_worldName);

    // publish clock for simulated ros time
    pub_clock_ = this->rosnode_->advertise<rosgraph_msgs::clock>("/clock",10);
    // set param for use_sim_time if not set by user alread
    this->rosnode_->setParam("/use_sim_time", true);

    this->time_update_event_   = event::Events::ConnectWorldUpdateStart(boost::bind(&SystemClock::publishSimTime,this));
}



////////////////////////////////////////////////////////////////////////////////
/// \brief
void SystemClock::publishSimTime(const boost::shared_ptr<msgs::worldstatistics const=""> &msg)
{
    ROS_ERROR("CLOCK2");
    common::Time currentTime = msgs::Convert( msg->sim_time() );
    rosgraph_msgs::Clock ros_time_;
    ros_time_.clock.fromSec(currentTime.Double());
    //  publish time to ros
    this->pub_clock_.publish(ros_time_);
}
void SystemClock::publishSimTime()
{
    common::Time currentTime = this->world->GetSimTime();
    rosgraph_msgs::Clock ros_time_;
    ros_time_.clock.fromSec(currentTime.Double());
    //  publish time to ros
    this->pub_clock_.publish(ros_time_);
}

// Register this plugin with the simulator
GZ_REGISTER_SYSTEM_PLUGIN(SystemClock)

}