StageNode::StageNode(int argc, char** argv, bool gui, const char* fname)
{
this->sim_time.fromSec(0.0);
this->base_last_cmd.fromSec(0.0);
double t;
ros::NodeHandle localn("~");
if(!localn.getParam("base_watchdog_timeout", t))
t = 0.2;
this->base_watchdog_timeout.fromSec(t);
// We'll check the existence of the world file, because libstage doesn't
// expose its failure to open it. Could go further with checks (e.g., is
// it readable by this user).
struct stat s;
if(stat(fname, &s) != 0)
{
ROS_FATAL("The world file %s does not exist.", fname);
ROS_BREAK();
}
// initialize libstage
Stg::Init( &argc, &argv );
if(gui)
this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
else
this->world = new Stg::World();
// Apparently an Update is needed before the Load to avoid crashes on
// startup on some systems.
// As of Stage 4.1.1, this update call causes a hang on start.
//this->UpdateWorld();
this->world->Load(fname);
// We add our callback here, after the Update, so avoid our callback
// being invoked before we're ready.
this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);
this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
if (lasermodels.size() != positionmodels.size())
{
ROS_FATAL("number of position models and laser models must be equal in "
"the world file.");
ROS_BREAK();
}
size_t numRobots = positionmodels.size();
ROS_INFO("found %u position/laser pair%s in the file",
(unsigned int)numRobots, (numRobots==1) ? "" : "s");
this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
this->odomMsgs = new nav_msgs::Odometry[numRobots];
this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
this->colorMsgs = new my_stage::Color[numRobots];
}
StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names)
{
this->use_model_names = use_model_names;
this->sim_time.fromSec(0.0);
this->base_last_cmd.fromSec(0.0);
double t;
ros::NodeHandle localn("~");
if(!localn.getParam("base_watchdog_timeout", t))
t = 0.2;
this->base_watchdog_timeout.fromSec(t);
if(!localn.getParam("is_depth_canonical", isDepthCanonical))
isDepthCanonical = true;
// We'll check the existence of the world file, because libstage doesn't
// expose its failure to open it. Could go further with checks (e.g., is
// it readable by this user).
struct stat s;
if(stat(fname, &s) != 0)
{
ROS_FATAL("The world file %s does not exist.", fname);
ROS_BREAK();
}
// initialize libstage
Stg::Init( &argc, &argv );
if(gui)
this->world = new Stg::WorldGui(600, 400, "Stage (ROS)");
else
this->world = new Stg::World();
// Apparently an Update is needed before the Load to avoid crashes on
// startup on some systems.
// As of Stage 4.1.1, this update call causes a hang on start.
//this->UpdateWorld();
this->world->Load(fname);
// We add our callback here, after the Update, so avoid our callback
// being invoked before we're ready.
this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);
this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
}
