Downgrade to the indigo-devel branch because we are on kinetic not lu…

…nar.

See ros-simulation/stage_ros#27 (comment).

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 <https://github.com/ros-simulation/stage_ros/issues/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.

src/stage_ros_mod_tf/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>stage_ros</name>
-  <version>1.8.0</version>
+  <version>1.7.5</version>
   <description>This package provides ROS specific hooks for stage</description>
 
   <maintainer email="[email protected]">William Woodall</maintainer>

src/stage_ros_mod_tf/src/stageros.cpp

@@ -37,7 +37,6 @@
 // libstage
 #include <stage.hh>
 
-
 // roscpp
 #include <ros/ros.h>
 #include <boost/thread/mutex.hpp>
@@ -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<Stg::ModelRanger *>(mod)) {
-     node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
-  }
-  if (dynamic_cast<Stg::ModelPosition *>(mod)) {
-     Stg::ModelPosition * p = dynamic_cast<Stg::ModelPosition *>(mod);
+    if (dynamic_cast<Stg::ModelRanger *>(mod))
+        node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
+    if (dynamic_cast<Stg::ModelPosition *>(mod)) {
+      Stg::ModelPosition * p = dynamic_cast<Stg::ModelPosition *>(mod);
       // remember initial poses
       node->positionmodels.push_back(p);
       node->initial_poses.push_back(p->GetGlobalPose());
     }
-  if (dynamic_cast<Stg::ModelCamera *>(mod)) {
-     node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
-  }
+    if (dynamic_cast<Stg::ModelCamera *>(mod))
+        node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(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<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
         new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(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);

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")