Revision: 7375
http://playerstage.svn.sourceforge.net/playerstage/?rev=7375&view=rev
Author: rtv
Date: 2009-03-08 01:44:36 +0000 (Sun, 08 Mar 2009)
Log Message:
-----------
initial Stage-Gazebo federation demo
Modified Paths:
--------------
code/branches/federation/stage/CMakeLists.txt
code/branches/federation/stage/examples/CMakeLists.txt
Added Paths:
-----------
code/branches/federation/stage/examples/gzfed/
code/branches/federation/stage/examples/gzfed/CMakeLists.txt
code/branches/federation/stage/examples/gzfed/gzwander.cc
code/branches/federation/stage/examples/gzfed/stage.world
Modified: code/branches/federation/stage/CMakeLists.txt
===================================================================
--- code/branches/federation/stage/CMakeLists.txt 2009-03-08 01:42:39 UTC
(rev 7374)
+++ code/branches/federation/stage/CMakeLists.txt 2009-03-08 01:44:36 UTC
(rev 7375)
@@ -59,6 +59,16 @@
MESSAGE( STATUS "Checking for required libraries..." )
SET( INDENT " * " )
+
+pkg_search_module( GAZEBO REQUIRED libgazebo )
+IF( GAZEBO_FOUND )
+ MESSAGE( STATUS ${INDENT} "Gazebo version ${GAZEBO_VERSION} detected at
${GAZEBO_PREFIX}" )
+ MESSAGE( STATUS " GAZEBO_CFLAGS = ${GAZEBO_CFLAGS}" )
+ MESSAGE( STATUS " GAZEBO_LDFLAGS = ${GAZEBO_LDFLAGS}" )
+ELSE( GAZEBO_FOUND )
+ MESSAGE( ${INDENT} "Gazebo not detected" )
+ENDIF( GAZEBO_FOUND )
+
pkg_search_module( GLIB REQUIRED gthread-2.0 )
IF( GLIB_FOUND )
MESSAGE( STATUS ${INDENT} "Glib version ${GLIB_VERSION} detected at
${GLIB_PREFIX}" )
@@ -174,6 +184,7 @@
${GLIB_INCLUDE_DIRS}
${LIBPNG_INCLUDE_DIRS}
${CMAKE_INCLUDE_PATH}
+ ${GAZEBO_INCLUDE_PATH}
)
@@ -181,6 +192,7 @@
link_directories(${GLIB_LIBRARY_DIRS}
${GLIB_LIBRARY_DIRS}
${LIBPNG_LIBRARY_DIRS}
+ ${GAZEBO_LIBRARY_DIRS}
)
# work through these subdirs
Modified: code/branches/federation/stage/examples/CMakeLists.txt
===================================================================
--- code/branches/federation/stage/examples/CMakeLists.txt 2009-03-08
01:42:39 UTC (rev 7374)
+++ code/branches/federation/stage/examples/CMakeLists.txt 2009-03-08
01:44:36 UTC (rev 7375)
@@ -1 +1,2 @@
ADD_SUBDIRECTORY(ctrl)
+ADD_SUBDIRECTORY(gzfed)
Added: code/branches/federation/stage/examples/gzfed/CMakeLists.txt
===================================================================
--- code/branches/federation/stage/examples/gzfed/CMakeLists.txt
(rev 0)
+++ code/branches/federation/stage/examples/gzfed/CMakeLists.txt
2009-03-08 01:44:36 UTC (rev 7375)
@@ -0,0 +1,18 @@
+
+SET( GZPLUGINS
+ gzwander
+)
+
+# create a library module for each plugin and link libstage and libgazebo to
each
+foreach( PLUGIN ${GZPLUGINS} )
+ ADD_LIBRARY( ${PLUGIN} MODULE ${PLUGIN}.cc )
+ TARGET_LINK_LIBRARIES( ${PLUGIN} stage ${GAZEBO_LIBRARIES})
+ set_source_files_properties( ${PLUGIN}.cc PROPERTIES COMPILE_FLAGS
"${GAZEBO_CFLAGS}")
+endforeach( PLUGIN )
+
+# delete the "lib" prefix from the plugin libraries
+SET_TARGET_PROPERTIES( ${GZPLUGINS} PROPERTIES PREFIX "" )
+
+# install in <prefix>/lib
+INSTALL( TARGETS ${GZPLUGINS} DESTINATION lib)
+
Added: code/branches/federation/stage/examples/gzfed/gzwander.cc
===================================================================
--- code/branches/federation/stage/examples/gzfed/gzwander.cc
(rev 0)
+++ code/branches/federation/stage/examples/gzfed/gzwander.cc 2009-03-08
01:44:36 UTC (rev 7375)
@@ -0,0 +1,235 @@
+
+// gazebo client library
+#include <gazebo/gazebo.h>
+#include <gazebo/GazeboError.hh>
+
+#include "stage.hh"
+using namespace Stg;
+
+const double cruisespeed = 0.4;
+const double avoidspeed = 0.05;
+const double avoidturn = 0.5;
+const double minfrontdistance = 0.6;
+const bool verbose = false;
+const double stopdist = 0.3;
+const int avoidduration = 10;
+
+gazebo::Client *gz_client = NULL;
+gazebo::SimulationIface *gz_simIface = NULL;
+
+typedef struct
+{
+ const char* gazeboid;
+ ModelPosition* pos;
+ ModelLaser* laser;
+ int avoidcount, randcount;
+} robot_t;
+
+int LaserUpdate( Model* mod, robot_t* robot );
+int PositionUpdate( Model* mod, robot_t* robot );
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod )
+{
+ gz_client = new gazebo::Client();
+ gz_simIface = new gazebo::SimulationIface();
+ assert( gz_client );
+ assert( gz_simIface );
+
+ int serverId = 0;
+
+ /// Connect to the libgazebo server
+ try
+ {
+ gz_client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
+ }
+ catch( std::string e)
+ {
+ std::cout << "libGazebo error: Unable to connect\n" << e <<
"\n";
+ return -1;
+ }
+
+ /// Open the Simulation Interface
+ try
+ {
+ gz_simIface->Open( gz_client, "default");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the sim
interface\n" << e << "\n";
+ return -1;
+ }
+
+
+ robot_t* robot = new robot_t;
+
+ robot->avoidcount = 0;
+ robot->randcount = 0;
+
+ robot->pos = (ModelPosition*)mod;
+ robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot );
+
+
+ robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
+ robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
+ robot->laser->Subscribe(); // starts the laser
+
+ robot->gazeboid = "pioneer2dx_model1";
+
+ return 0; //ok
+}
+
+
+// inspect the laser data and decide what to do
+int LaserUpdate( Model* mod, robot_t* robot )
+{
+ // get the data
+ uint32_t sample_count=0;
+ stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count );
+
+ if( ! scan )
+ return 0;
+
+ bool obstruction = false;
+ bool stop = false;
+
+ // find the closest distance to the left and right and check if
+ // there's anything in front
+ double minleft = 1e6;
+ double minright = 1e6;
+
+ for (uint32_t i = 0; i < sample_count; i++)
+ {
+
+ if( verbose ) printf( "%.3f ", scan[i].range );
+
+ if( (i > (sample_count/3))
+ && (i < (sample_count - (sample_count/3)))
+ && scan[i].range < minfrontdistance)
+ {
+ if( verbose ) puts( " obstruction!" );
+ obstruction = true;
+ }
+
+ if( scan[i].range < stopdist )
+ {
+ if( verbose ) puts( " stopping!" );
+ stop = true;
+ }
+
+ if( i > sample_count/2 )
+ minleft = MIN( minleft, scan[i].range );
+ else
+ minright = MIN( minright, scan[i].range );
+ }
+
+ if( verbose )
+ {
+ puts( "" );
+ printf( "minleft %.3f \n", minleft );
+ printf( "minright %.3f\n ", minright );
+ }
+
+ if( obstruction || stop || (robot->avoidcount>0) )
+ {
+ if( verbose ) printf( "Avoid %d\n", robot->avoidcount );
+
+ robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed );
+
+ /* once we start avoiding, select a turn direction and stick
+ with it for a few iterations */
+ if( robot->avoidcount < 1 )
+ {
+ if( verbose ) puts( "Avoid START" );
+ robot->avoidcount = random() % avoidduration + avoidduration;
+
+ if( minleft < minright )
+ {
+ robot->pos->SetTurnSpeed( -avoidturn );
+ if( verbose ) printf( "turning right %.2f\n",
-avoidturn );
+ }
+ else
+ {
+ robot->pos->SetTurnSpeed( +avoidturn );
+ if( verbose ) printf( "turning left %2f\n",
+avoidturn );
+ }
+ }
+
+ robot->avoidcount--;
+ }
+ else
+ {
+ if( verbose ) puts( "Cruise" );
+
+ robot->avoidcount = 0;
+ robot->pos->SetXSpeed( cruisespeed );
+ robot->pos->SetTurnSpeed( 0 );
+ }
+
+ return 0;
+}
+
+void gzPush( const char* name,
+ double x,
+ double y,
+ double z,
+ double roll,
+ double pitch,
+ double yaw,
+ double dx,
+ double dy,
+ double dz,
+ double droll,
+ double dpitch,
+ double dyaw )
+{
+ gz_simIface->Lock(1);
+
+ gazebo::SimulationRequestData *request =
&(gz_simIface->data->requests[gz_simIface->data->requestCount++]);
+
+ request->type = gazebo::SimulationRequestData::SET_STATE;
+ memcpy(request->modelName, name, 512);
+
+ request->modelPose.pos.x = x;
+ request->modelPose.pos.y = y;
+ request->modelPose.pos.z = z;
+ request->modelPose.roll = roll;
+ request->modelPose.pitch = pitch;
+ request->modelPose.yaw = yaw;
+
+ request->modelLinearVel.x = dx;
+ request->modelLinearVel.y = dy;
+ request->modelLinearVel.z = dz;
+
+ request->modelAngularVel.x = droll;
+ request->modelAngularVel.y = dpitch;
+ request->modelAngularVel.z = dyaw;
+
+ gz_simIface->Unlock();
+}
+
+
+int PositionUpdate( Model* mod, robot_t* robot )
+{
+ Pose pose = robot->pos->GetPose();
+ Velocity vel = robot->pos->GetVelocity();
+
+ printf( "P [%.2f %.2f %.2f %.2f] V [%.2f %.2f %.2f %.2f]\n",
+ pose.x, pose.y, pose.z, pose.a,
+ vel.x, vel.y, vel.z, vel.a );
+
+
+ static int skip = 100;
+
+ if( --skip == 0 )
+ {
+ // tell gazebo the new position
+ gzPush( robot->gazeboid,
+ pose.x, pose.y, pose.z+0.146, 0,0, pose.a,
+ vel.x, vel.y, vel.z, 0,0, vel.a );
+
+ skip = 100;
+ }
+
+ return 0; // run again
+}
Added: code/branches/federation/stage/examples/gzfed/stage.world
===================================================================
--- code/branches/federation/stage/examples/gzfed/stage.world
(rev 0)
+++ code/branches/federation/stage/examples/gzfed/stage.world 2009-03-08
01:44:36 UTC (rev 7375)
@@ -0,0 +1,48 @@
+# simple.world - basic world file example
+# Authors: Richard Vaughan
+# $Id$
+
+include "../../worlds/pioneer.inc"
+include "../../worlds/map.inc"
+include "../../worlds/sick.inc"
+
+interval_sim 100 # simulation timestep in milliseconds
+interval_real 10 # real-time interval between simulation updates in
milliseconds
+
+paused 0
+
+resolution 0.02
+
+# configure the GUI window
+window
+(
+ size [ 556.000 557.000 ] # in pixels
+ scale 28.116
+ # pixels per meter
+ center [ 8.058 7.757 ]
+ rotate [ 0 0 ]
+
+ show_data 1 # 1=on 0=off
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "cave"
+ size [16.000 16.000 0.800]
+ pose [8 8 0 0]
+ bitmap "../../worlds/bitmaps/cave.png"
+)
+
+
+pioneer2dx
+(
+ # can refer to the robot by this name
+ name "r0"
+
+ pose [ 0.892 0.800 0 56.500 ]
+ sicklaser()
+
+ ctrl "gzwander"
+)
+
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
