void WorldRosItem::start()
{
static bool initialized = false;
if (initialized) return;
world = this->findOwnerItem<WorldItem>();
if (world)
ROS_INFO("Found WorldItem: %s", world->name().c_str());
sim = SimulatorItem::findActiveSimulatorItemFor(this);
if (sim == 0) return;
std::string name = sim->name();
ROS_INFO("Found SimulatorItem: %s", name.c_str());
std::replace(name.begin(), name.end(), '-', '_');
rosnode_ = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(world->name()));
pub_clock_ = rosnode_->advertise<rosgraph_msgs::Clock>("/clock", 10);
pub_link_states_ = rosnode_->advertise<gazebo_msgs::LinkStates>("link_states", 10);
pub_model_states_ = rosnode_->advertise<gazebo_msgs::ModelStates>("model_states", 10);
TimeBar* timeBar = TimeBar::instance();
timeBar->sigTimeChanged().connect(boost::bind(&WorldRosItem::timeTick, this, _1));
std::string pause_physics_service_name("pause_physics");
ros::AdvertiseServiceOptions pause_physics_aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
pause_physics_service_name,
boost::bind(&WorldRosItem::pausePhysics,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
pause_physics_service_ = rosnode_->advertiseService(pause_physics_aso);
std::string unpause_physics_service_name("unpause_physics");
ros::AdvertiseServiceOptions unpause_physics_aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
unpause_physics_service_name,
boost::bind(&WorldRosItem::unpausePhysics,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
unpause_physics_service_ = rosnode_->advertiseService(unpause_physics_aso);
std::string reset_simulation_service_name("reset_simulation");
ros::AdvertiseServiceOptions reset_simulation_aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
reset_simulation_service_name,
boost::bind(&WorldRosItem::resetSimulation,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
reset_simulation_service_ = rosnode_->advertiseService(reset_simulation_aso);
std::string spawn_vrml_model_service_name("spawn_vrml_model");
ros::AdvertiseServiceOptions spawn_vrml_model_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
spawn_vrml_model_service_name,
boost::bind(&WorldRosItem::spawnModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
spawn_vrml_model_service_ = rosnode_->advertiseService(spawn_vrml_model_aso);
std::string spawn_urdf_model_service_name("spawn_urdf_model");
ros::AdvertiseServiceOptions spawn_urdf_model_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
spawn_urdf_model_service_name,
boost::bind(&WorldRosItem::spawnModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
spawn_urdf_model_service_ = rosnode_->advertiseService(spawn_urdf_model_aso);
std::string spawn_sdf_model_service_name("spawn_sdf_model");
ros::AdvertiseServiceOptions spawn_sdf_model_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
spawn_sdf_model_service_name,
boost::bind(&WorldRosItem::spawnModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
spawn_sdf_model_service_ = rosnode_->advertiseService(spawn_sdf_model_aso);
std::string delete_model_service_name("delete_model");
ros::AdvertiseServiceOptions delete_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::DeleteModel>(
delete_model_service_name,
boost::bind(&WorldRosItem::deleteModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
delete_model_service_ = rosnode_->advertiseService(delete_aso);
async_ros_spin_.reset(new ros::AsyncSpinner(0));
async_ros_spin_->start();
rosqueue_thread_.reset(new boost::thread(&WorldRosItem::queueThread, this));
initialized = true;
}
