diff --git a/src/stage_ros_mod_tf/CHANGELOG.rst b/src/stage_ros_mod_tf/CHANGELOG.rst index 4b8b351..098b70a 100644 --- a/src/stage_ros_mod_tf/CHANGELOG.rst +++ b/src/stage_ros_mod_tf/CHANGELOG.rst @@ -2,14 +2,6 @@ Changelog for package stage_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.8.0 (2017-04-30) ------------------- -* Now uses Stage's native event loop properly and added reassuring startup output. -* Added a GUI section so that the world starts in a good place. -* Fixed issue such that ranger intensity values are no longer clipped to 256 - See: `#31 `_ -* Contributors: Richard Vaughan, Shane Loretz, William Woodall, gerkey - 1.7.5 (2015-09-16) ------------------ * Removed all references to FLTK/Fluid and use the upstream CMake config file instead. diff --git a/src/stage_ros_mod_tf/package.xml b/src/stage_ros_mod_tf/package.xml index ef3ccdf..8803254 100644 --- a/src/stage_ros_mod_tf/package.xml +++ b/src/stage_ros_mod_tf/package.xml @@ -1,7 +1,7 @@ stage_ros - 1.8.0 + 1.7.5 This package provides ROS specific hooks for stage William Woodall diff --git a/src/stage_ros_mod_tf/src/stageros.cpp b/src/stage_ros_mod_tf/src/stageros.cpp index 39d040a..b6a5869 100644 --- a/src/stage_ros_mod_tf/src/stageros.cpp +++ b/src/stage_ros_mod_tf/src/stageros.cpp @@ -37,7 +37,6 @@ // libstage #include - // roscpp #include #include @@ -229,20 +228,16 @@ StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model void StageNode::ghfunc(Stg::Model* mod, StageNode* node) { - //printf( "inspecting %s, parent\n", mod->Token() ); - - if (dynamic_cast(mod)) { - node->lasermodels.push_back(dynamic_cast(mod)); - } - if (dynamic_cast(mod)) { - Stg::ModelPosition * p = dynamic_cast(mod); + if (dynamic_cast(mod)) + node->lasermodels.push_back(dynamic_cast(mod)); + if (dynamic_cast(mod)) { + Stg::ModelPosition * p = dynamic_cast(mod); // remember initial poses node->positionmodels.push_back(p); node->initial_poses.push_back(p->GetGlobalPose()); } - if (dynamic_cast(mod)) { - node->cameramodels.push_back(dynamic_cast(mod)); - } + if (dynamic_cast(mod)) + node->cameramodels.push_back(dynamic_cast(mod)); } @@ -285,6 +280,7 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us 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). @@ -303,13 +299,16 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us 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); - // todo: reverse the order of these next lines? try it . - + // 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); - // inspect every model to locate the things we care about this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this); } @@ -331,15 +330,13 @@ StageNode::SubscribeModels() new_robot->positionmodel = this->positionmodels[r]; new_robot->positionmodel->Subscribe(); - ROS_INFO( "Subscribed to Stage position model \"%s\"", this->positionmodels[r]->Token() ); - + for (size_t s = 0; s < this->lasermodels.size(); s++) { - if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel) + if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel) { new_robot->lasermodels.push_back(this->lasermodels[s]); this->lasermodels[s]->Subscribe(); - ROS_INFO( "subscribed to Stage ranger \"%s\"", this->lasermodels[s]->Token() ); } } @@ -349,16 +346,10 @@ StageNode::SubscribeModels() { new_robot->cameramodels.push_back(this->cameramodels[s]); this->cameramodels[s]->Subscribe(); - - ROS_INFO( "subscribed to Stage camera model \"%s\"", this->cameramodels[s]->Token() ); } } - // TODO - print the topic names nicely as well - ROS_INFO("Robot %s provided %lu rangers and %lu cameras", - new_robot->positionmodel->Token(), - new_robot->lasermodels.size(), - new_robot->cameramodels.size() ); + ROS_INFO("Found %lu laser devices and %lu cameras in robot %lu", new_robot->lasermodels.size(), new_robot->cameramodels.size(), r); new_robot->odom_pub = n_.advertise(mapName(ODOM, r, static_cast(new_robot->positionmodel)), 10); new_robot->ground_truth_pub = n_.advertise(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast(new_robot->positionmodel)), 10); @@ -414,12 +405,6 @@ StageNode::UpdateWorld() void StageNode::WorldCallback() { - if( ! ros::ok() ) { - ROS_INFO( "ros::ok() is false. Quitting." ); - this->world->QuitAll(); - return; - } - boost::mutex::scoped_lock lock(msg_lock); this->sim_time.fromSec(world->SimTimeNow() / 1e6); @@ -748,7 +733,7 @@ StageNode::WorldCallback() int main(int argc, char** argv) -{ +{ if( argc < 2 ) { puts(USAGE); @@ -774,10 +759,22 @@ main(int argc, char** argv) boost::thread t = boost::thread(boost::bind(&ros::spin)); + // New in Stage 4.1.1: must Start() the world. sn.world->Start(); - Stg::World::Run(); - + // TODO: get rid of this fixed-duration sleep, using some Stage builtin + // PauseUntilNextUpdate() functionality. + ros::WallRate r(10.0); + while(ros::ok() && !sn.world->TestQuit()) + { + if(gui) + Fl::wait(r.expectedCycleTime().toSec()); + else + { + sn.UpdateWorld(); + r.sleep(); + } + } t.join(); exit(0); diff --git a/src/stage_ros_mod_tf/world/willow-erratic.world b/src/stage_ros_mod_tf/world/willow-erratic.world index 6e341d9..4502bb4 100644 --- a/src/stage_ros_mod_tf/world/willow-erratic.world +++ b/src/stage_ros_mod_tf/world/willow-erratic.world @@ -1,17 +1,6 @@ -window -( - size [ 635 666 ] # in pixels - scale 22.971 # pixels per meter - center [ -20.306 21.679 ] - rotate [ 0.000 0.000 ] - - show_data 1 # 1=on 0=off -) - - define block model ( - size [0.500 0.500 0.500] + size [0.5 0.5 0.5] gui_nose 0 ) @@ -25,17 +14,17 @@ define topurg ranger # generic model properties color "black" - size [ 0.050 0.050 0.100 ] + size [ 0.05 0.05 0.1 ] ) define erratic position ( #size [0.415 0.392 0.25] - size [0.350 0.350 0.250] - origin [-0.050 0.000 0.000 0.000] + size [0.35 0.35 0.25] + origin [-0.05 0 0 0] gui_nose 1 drive "diff" - topurg(pose [ 0.050 0.000 0.000 0.000 ]) + topurg(pose [ 0.050 0.000 0 0.000 ]) ) define floorplan model @@ -52,7 +41,7 @@ define floorplan model gui_outline 0 gripper_return 0 fiducial_return 0 - ranger_return 1.000 + laser_return 1 ) # set the resolution of the underlying raytrace model in meters @@ -63,7 +52,7 @@ interval_sim 100 # simulation timestep in milliseconds window ( - size [ 745 448 ] + size [ 745.000 448.000 ] rotate [ 0.000 -1.560 ] scale 28.806 @@ -74,10 +63,10 @@ floorplan ( name "willow" bitmap "willow-full.pgm" - size [54.000 58.700 0.500] - pose [ -29.350 27.000 0.000 90.000 ] + size [54.0 58.7 0.5] + pose [ -29.350 27.000 0 90.000 ] ) # throw in a robot -erratic( pose [ -11.277 23.266 0.000 180.000 ] name "era" color "blue") -block( pose [ -13.924 25.020 0.000 180.000 ] color "red") +erratic( pose [ -11.277 23.266 0 180.000 ] name "era" color "blue") +block( pose [ -13.924 25.020 0 180.000 ] color "red")