[Personalrobots-commit] SF.net SVN: personalrobots: [390] pkg/trunk

[<ge...@us...>]

Revision: 390
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=390&view=rev
Author:   gerkey
Date:     2008-05-11 17:13:41 -0700 (Sun, 11 May 2008)

Log Message:
-----------
added ros wrapper for stage

Added Paths:
-----------
    pkg/trunk/simulators/
    pkg/trunk/simulators/rosstage/
    pkg/trunk/simulators/rosstage/.player
    pkg/trunk/simulators/rosstage/Makefile
    pkg/trunk/simulators/rosstage/manifest.xml
    pkg/trunk/simulators/rosstage/rosstage.cc
    pkg/trunk/simulators/rosstage/willow-erratic.world
    pkg/trunk/simulators/rosstage/willow-full.png

Added: pkg/trunk/simulators/rosstage/.player
===================================================================
--- pkg/trunk/simulators/rosstage/.player	                        (rev 0)
+++ pkg/trunk/simulators/rosstage/.player	2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,21 @@
+plugins.cc:160 trying to load /Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin...
+plugins.cc:164 failed (dlopen(/Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin.so, 9): image not found)
+
+plugins.cc:175 trying to load /Users/gerkey/ps-3.0/lib/libstageplugin...
+plugins.cc:177 success
+plugins.cc:217 invoking player_driver_init()...
+plugins.cc:233 success
+plugins.cc:160 trying to load /Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin...
+plugins.cc:164 failed (dlopen(/Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin.so, 9): image not found)
+
+plugins.cc:175 trying to load /Users/gerkey/ps-2.1/lib/libstageplugin...
+plugins.cc:177 success
+plugins.cc:217 invoking player_driver_init()...
+plugins.cc:233 success
+plugins.cc:160 trying to load /Users/gerkey/ps-2.1/share/stage/worlds/libstageplugin...
+plugins.cc:164 failed (dlopen(/Users/gerkey/ps-2.1/share/stage/worlds/libstageplugin.so, 9): image not found)
+
+plugins.cc:175 trying to load /Users/gerkey/ps-2.1/lib/libstageplugin...
+plugins.cc:177 success
+plugins.cc:217 invoking player_driver_init()...
+plugins.cc:233 success

Added: pkg/trunk/simulators/rosstage/Makefile
===================================================================
--- pkg/trunk/simulators/rosstage/Makefile	                        (rev 0)
+++ pkg/trunk/simulators/rosstage/Makefile	2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,12 @@
+PKG = rosstage
+CXX = g++
+all: $(PKG)
+
+CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
+
+rosstage: rosstage.cc
+	$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
+
+clean:
+	rm -rf *.o $(PKG) $(PKG).dSYM

Added: pkg/trunk/simulators/rosstage/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosstage/manifest.xml	                        (rev 0)
+++ pkg/trunk/simulators/rosstage/manifest.xml	2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,8 @@
+<package>
+  <description>A ROS node that wraps up the Stage mobile robot simulator.</description>
+  <author>Brian P. Gerkey</author>
+  <license>GPL</license>
+  <depend package="roscpp" />
+  <depend package="stage" />
+  <depend package="std_msgs" />
+</package>

Added: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc	                        (rev 0)
+++ pkg/trunk/simulators/rosstage/rosstage.cc	2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,172 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+// libstage
+#include <stage.hh>
+
+// roscpp
+#include <ros/node.h>
+#include <std_msgs/MsgLaserScan.h>
+#include <std_msgs/MsgRobotBase2DOdom.h>
+#include <std_msgs/MsgBaseVel.h>
+
+#define USAGE "rosstage <worldfile>"
+
+class StageNode : public ros::node
+{
+  private:
+    MsgBaseVel velMsg;
+    MsgLaserScan laserMsg;
+    MsgRobotBase2DOdom odomMsg;
+
+  public:
+    StageNode(int argc, char** argv, const char* fname);
+    ~StageNode();
+    int SubscribeModels();
+    void cmdvelReceived();
+    void Update();
+
+    static void ghfunc(gpointer key, Stg::StgModel* mod, StageNode* node);
+
+    Stg::StgWorldGui* world;
+    Stg::StgModelLaser* lasermodel;
+    Stg::StgModelPosition* positionmodel;
+};
+
+void
+StageNode::ghfunc(gpointer key,
+                  Stg::StgModel* mod,
+                  StageNode* node)
+{
+  if(!(node->lasermodel) &&
+     (node->lasermodel = dynamic_cast<Stg::StgModelLaser*>(mod)))
+  {
+    puts("found laser");
+  }
+  if(!(node->positionmodel) &&
+     (node->positionmodel = dynamic_cast<Stg::StgModelPosition*>(mod)))
+  {
+    puts("found position");
+  }
+}
+
+void
+StageNode::cmdvelReceived()
+{
+  printf("received cmd: %.3f %.3f\n",
+         this->velMsg.vx, this->velMsg.vw);
+}
+
+StageNode::StageNode(int argc, char** argv, const char* fname) :
+        ros::node("rosstage")
+{
+  this->lasermodel = NULL;
+  this->positionmodel = NULL;
+
+  // initialize libstage
+  Stg::Init( &argc, &argv );
+  //StgWorld world;
+  this->world = new Stg::StgWorldGui(800, 700, "Stage (ROS)");
+
+  this->world->Load(fname);
+
+  this->world->ForEachModel((GHFunc)ghfunc, this);
+}
+
+int
+StageNode::SubscribeModels()
+{
+  if(this->lasermodel)
+    this->lasermodel->Subscribe();
+  else
+  {
+    puts("no laser");
+    return(-1);
+  }
+  if(this->positionmodel)
+    this->positionmodel->Subscribe();
+  else
+  {
+    puts("no position");
+    return(-1);
+  }
+
+  advertise<MsgLaserScan>("scan");
+  advertise<MsgRobotBase2DOdom>("odom");
+  subscribe("cmd_vel", velMsg, &StageNode::cmdvelReceived);
+  return(0);
+}
+
+StageNode::~StageNode()
+{
+}
+
+void
+StageNode::Update()
+{
+  if(this->world->RealTimeUpdate())
+  {
+    Stg::stg_laser_sample_t* samples = this->lasermodel->GetSamples();
+    if(samples)
+    {
+      Stg::stg_laser_cfg_t cfg = this->lasermodel->GetConfig();
+      this->laserMsg.angle_min = -cfg.fov/2.0;
+      this->laserMsg.angle_max = +cfg.fov/2.0;
+      this->laserMsg.angle_increment = cfg.fov / (double)(cfg.sample_count-1);
+      this->laserMsg.range_max = cfg.range_bounds.max;
+      this->laserMsg.set_ranges_size(cfg.sample_count);
+      this->laserMsg.set_intensities_size(cfg.sample_count);
+      for(unsigned int i=0;i<cfg.sample_count;i++)
+      {
+        this->laserMsg.ranges[i] = samples[i].range;
+        this->laserMsg.intensities[i] = (uint8_t)samples[i].reflectance;
+      }
+
+      publish("scan",this->laserMsg);
+    }
+
+    this->odomMsg.pos.x = this->positionmodel->est_pose.x;
+    this->odomMsg.pos.y = this->positionmodel->est_pose.y;
+    this->odomMsg.pos.th = this->positionmodel->est_pose.a;
+
+    Stg::stg_velocity_t v = this->positionmodel->GetVelocity();
+
+    this->odomMsg.vel.x = v.x;
+    this->odomMsg.vel.y = v.y;
+    this->odomMsg.vel.th = v.a;
+
+    this->odomMsg.stall = this->positionmodel->Stall();
+
+    publish("odom",this->odomMsg);
+
+  }
+}
+
+int 
+main(int argc, char** argv)
+{ 
+  if( argc < 2 )
+  {
+    puts(USAGE);
+    exit(-1);
+  }
+
+  ros::init(argc,argv);
+
+  StageNode sn(argc,argv,argv[1]);
+
+  if(sn.SubscribeModels() != 0)
+    exit(-1);
+
+  while(sn.ok() && !sn.world->TestQuit())
+  {
+    sn.Update();
+  }
+  
+  // have to call this explicitly for some reason.  probably interference
+  // from signal handling in Stage?
+  ros::msg_destruct();
+
+  exit(0);
+}

Added: pkg/trunk/simulators/rosstage/willow-erratic.world
===================================================================
--- pkg/trunk/simulators/rosstage/willow-erratic.world	                        (rev 0)
+++ pkg/trunk/simulators/rosstage/willow-erratic.world	2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,64 @@
+define topurg laser
+(
+  range_min 0.0
+  range_max 30.0
+  fov 282.0
+  samples 1128
+
+  # generic model properties
+  color "black"
+  size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+  size3 [0.415 0.392 0.25]
+  origin3 [-0.05 0 0 0]
+  gui_nose 1
+  drive "diff"
+  topurg(pose [0.050 0.000 0.000])
+  color "blue"
+)
+
+define floorplan model
+(
+  # sombre, sensible, artistic
+  color "gray30"
+
+  # most maps will need a bounding box
+  boundary 1
+
+  gui_nose 0
+  gui_grid 0
+  gui_movemask 0
+  gui_outline 0
+  gripper_return 0
+  fiducial_return 0
+  laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.02
+
+interval_sim 100  # simulation timestep in milliseconds
+interval_real 100  # real-time interval between simulation updates in milliseconds 
+
+window
+( 
+  size [ 745.000 448.000 ] 
+  center [-7.010 5.960] 
+  rotate [ 0.000 -1.560 ]
+  scale 28.806 
+)
+
+# load an environment bitmap
+floorplan
+( 
+  name "willow"
+  bitmap "willow-full.png"
+  size3 [54.0 58.7 0.5]
+  pose [0.000 0.000 0.000]
+)
+
+# throw in a robot
+erratic(pose [0.000 0.000 0.000] name "era")

Added: pkg/trunk/simulators/rosstage/willow-full.png
===================================================================
(Binary files differ)


Property changes on: pkg/trunk/simulators/rosstage/willow-full.png
___________________________________________________________________
Name: svn:mime-type
   + application/octet-stream


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.