Revision: 8585
http://playerstage.svn.sourceforge.net/playerstage/?rev=8585&view=rev
Author: rtv
Date: 2010-03-12 02:24:27 +0000 (Fri, 12 Mar 2010)
Log Message:
-----------
added movable sources
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr2.cc
code/stage/trunk/examples/ctrl/source.cc
code/stage/trunk/libstage/color.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/worlds/fasr2.world
Modified: code/stage/trunk/examples/ctrl/fasr2.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-12 02:05:17 UTC (rev
8584)
+++ code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-12 02:24:27 UTC (rev
8585)
@@ -12,7 +12,6 @@
const double minfrontdistance = 0.7;
const double stopdist = 0.5;
const int avoidduration = 10;
-//const int workduration = 20;
const int PAYLOAD = 1;
//const int TRAIL_LENGTH_MAX = 500;
@@ -218,6 +217,7 @@
static pthread_mutex_t planner_mutex;
Model* goal;
+ Pose cached_goal_pose;
Graph graph;
GraphVis graphvis;
@@ -257,6 +257,7 @@
mode(MODE_WORK),
docked_angle(0),
goal(source),
+ cached_goal_pose(),
graph(),
graphvis( graph ),
last_node( NULL ),
@@ -651,7 +652,10 @@
if( Hungry() )
SetGoal( fuel_zone );
- while( graph.GoodDirection( pose, 4.0, a_goal ) == 0 )
+ // if the graph failes to offer advice or the goal has moved a
+ // ways since last time we planned
+ if( (graph.GoodDirection( pose, 4.0, a_goal ) == 0) ||
+ (goal->GetPose().Distance2D( cached_goal_pose )
> 2.0) )
{
//printf( "%s replanning from (%.2f,%.2f) to %s
at (%.2f,%.2f) in Work()\n",
// pos->Token(),
@@ -660,6 +664,7 @@
// goal->GetPose().x,
// goal->GetPose().y );
Plan( goal );
+ cached_goal_pose = goal->GetPose();
}
// if we are low on juice - find the direction to the
recharger instead
Modified: code/stage/trunk/examples/ctrl/source.cc
===================================================================
--- code/stage/trunk/examples/ctrl/source.cc 2010-03-12 02:05:17 UTC (rev
8584)
+++ code/stage/trunk/examples/ctrl/source.cc 2010-03-12 02:24:27 UTC (rev
8585)
@@ -2,7 +2,7 @@
using namespace Stg;
const int INTERVAL = 100;
-const double FLAGSZ = 0.4;
+const double FLAGSZ = 0.25;
const unsigned int CAPACITY = 1;
int Update( Model* mod, void* dummy );
Modified: code/stage/trunk/libstage/color.cc
===================================================================
--- code/stage/trunk/libstage/color.cc 2010-03-12 02:05:17 UTC (rev 8584)
+++ code/stage/trunk/libstage/color.cc 2010-03-12 02:24:27 UTC (rev 8585)
@@ -12,7 +12,7 @@
r(1.0), g(0.0), b(0.0), a(1.0)
{}
-bool Color::operator!=( const Color& other )
+bool Color::operator!=( const Color& other ) const
{
double epsilon = 1e-4; // small
return( fabs(r-other.r) > epsilon ||
@@ -84,7 +84,7 @@
this->a = found.a;
}
-bool Color::operator==( const Color& other )
+bool Color::operator==( const Color& other ) const
{
return( ! ((*this) != other) );
}
@@ -94,7 +94,7 @@
return Color( drand48(), drand48(), drand48() );
}
-void Color::Print( const char* prefix )
+void Color::Print( const char* prefix ) const
{
printf( "%s [%.2f %.2f %.2f %.2f]\n", prefix, r,g,b,a );
}
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2010-03-12 02:05:17 UTC (rev 8584)
+++ code/stage/trunk/libstage/stage.hh 2010-03-12 02:24:27 UTC (rev 8585)
@@ -222,10 +222,10 @@
Color();
- bool operator!=( const Color& other );
- bool operator==( const Color& other );
+ bool operator!=( const Color& other ) const;
+ bool operator==( const Color& other ) const;
static Color RandomColor();
- void Print( const char* prefix );
+ void Print( const char* prefix ) const;
};
/** specify a rectangular size */
@@ -284,13 +284,13 @@
/** Print pose in human-readable format on stdout
@param prefix Character string to prepend to pose output
*/
- virtual void Print( const char* prefix )
+ virtual void Print( const char* prefix ) const
{
printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
prefix, x,y,z,a );
}
- std::string String()
+ std::string String() const
{
char buf[256];
snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
@@ -299,15 +299,17 @@
}
/* returns true iff all components of the velocity are zero. */
- bool IsZero() const { return( !(x || y || z || a )); };
+ bool IsZero() const
+ { return( !(x || y || z || a )); };
/** Set the pose to zero [0,0,0,0] */
- void Zero(){ x=y=z=a=0.0; }
+ void Zero()
+ { x=y=z=a=0.0; }
void Load( Worldfile* wf, int section, const char* keyword );
void Save( Worldfile* wf, int section, const char* keyword );
- inline Pose operator+( const Pose& p )
+ inline Pose operator+( const Pose& p ) const
{
const double cosa = cos(a);
const double sina = sin(a);
@@ -323,6 +325,27 @@
{
return( hypot( y, x ) < hypot( other.y, other.x ));
}
+
+ bool operator==( const Pose& other ) const
+ {
+ return( x==other.x &&
+ y==other.y &&
+ z==other.z &&
+ a==other.a );
+ }
+
+ bool operator!=( const Pose& other ) const
+ {
+ return( x!=other.x ||
+ y!=other.y ||
+ z!=other.z ||
+ a!=other.a );
+ }
+
+ stg_meters_t Distance2D( const Pose& other ) const
+ {
+ return hypot( x-other.x, y-other.y );
+ }
};
Modified: code/stage/trunk/worlds/fasr2.world
===================================================================
--- code/stage/trunk/worlds/fasr2.world 2010-03-12 02:05:17 UTC (rev 8584)
+++ code/stage/trunk/worlds/fasr2.world 2010-03-12 02:24:27 UTC (rev 8585)
@@ -11,9 +11,9 @@
paused 1
# time at which to pause (in GUI mode) or quit (in headless mode) the
simulation
-quit_time 3600 # 1 hour of simulated time
-#quit_time 1800 # hour of simulated time
+# quit_time 3600 # 1 hour of simulated time
+
resolution 0.02
threads 8
@@ -23,9 +23,9 @@
(
size [ 1226.000 1080.000 ]
- center [ -41.006 -43.393 ]
+ center [ -40.702 -47.522 ]
rotate [ 0 0 ]
- scale 53.216
+ scale 22.245
show_data 1
show_flags 1
@@ -145,68 +145,40 @@
)
-charge_station( pose [ -43.500 -49.000 0 90.000 ] )
-charge_station( pose [ -37.500 -49.000 0 90.000 ] )
-charge_station( pose [ -39.500 -49.000 0 90.000 ] )
-charge_station( pose [ -41.500 -49.000 0 90.000 ] )
-charge_station( pose [ -38.500 -49.000 0 90.000 ] )
-charge_station( pose [ -40.500 -49.000 0 90.000 ] )
-charge_station( pose [ -42.500 -49.000 0 90.000 ] )
-charge_station( pose [ -47.500 -49.000 0 90.000 ] )
-charge_station( pose [ -46.500 -49.000 0 90.000 ] )
-charge_station( pose [ -44.500 -49.000 0 90.000 ] )
-charge_station( pose [ -45.500 -49.000 0 90.000 ] )
-charge_station( pose [ -37.000 -49.000 0 90.000 ] )
-charge_station( pose [ -38.000 -49.000 0 90.000 ] )
-charge_station( pose [ -39.000 -49.000 0 90.000 ] )
-charge_station( pose [ -41.000 -49.000 0 90.000 ] )
-charge_station( pose [ -40.000 -49.000 0 90.000 ] )
-charge_station( pose [ -42.000 -49.000 0 90.000 ] )
-charge_station( pose [ -43.000 -49.000 0 90.000 ] )
-charge_station( pose [ -44.000 -49.006 0 90.000 ] )
-charge_station( pose [ -45.000 -49.000 0 90.000 ] )
-charge_station( pose [ -46.000 -49.007 0 90.000 ] )
-charge_station( pose [ -47.000 -49.000 0 90.000 ] )
-charge_station( pose [ -49.000 -47.500 0 0 ] )
-charge_station( pose [ -49.000 -46.500 0 0 ] )
-charge_station( pose [ -49.000 -45.500 0 0 ] )
-charge_station( pose [ -49.000 -44.500 0 0 ] )
-charge_station( pose [ -49.000 -43.500 0 0 ] )
-charge_station( pose [ -49.000 -42.500 0 0 ] )
-charge_station( pose [ -49.000 -41.500 0 0 ] )
-charge_station( pose [ -49.000 -40.500 0 0 ] )
-charge_station( pose [ -49.000 -39.500 0 0 ] )
-charge_station( pose [ -49.000 -38.500 0 0 ] )
-charge_station( pose [ -49.000 -37.500 0 0 ] )
-charge_station( pose [ -49.000 -36.500 0 0 ] )
+charge_station( pose [ -38.500 -49.9 0 90.000 ] )
+charge_station( pose [ -39.500 -49.9 0 90.000 ] )
+charge_station( pose [ -40.500 -49.9 0 90.000 ] )
+charge_station( pose [ -41.500 -49.9 0 90.000 ] )
+charge_station( pose [ -42.500 -49.9 0 90.000 ] )
+charge_station( pose [ -43.500 -49.9 0 90.000 ] )
+charge_station( pose [ -44.500 -49.9 0 90.000 ] )
+charge_station( pose [ -45.500 -49.9 0 90.000 ] )
+charge_station( pose [ -46.500 -49.9 0 90.000 ] )
+charge_station( pose [ -47.500 -49.9 0 90.000 ] )
-#charge_station( pose [ -49.000 -36.00 0 0 ] )
-#charge_station( pose [ -49.000 -36.50 0 0 ] )
-charge_station( pose [ -49.000 -37.000 0 0 ] )
-charge_station( pose [ -49.000 -37.500 0 0 ] )
-charge_station( pose [ -49.000 -38.000 0 0 ] )
-charge_station( pose [ -49.000 -38.500 0 0 ] )
-charge_station( pose [ -49.000 -39.000 0 0 ] )
-charge_station( pose [ -49.000 -39.500 0 0 ] )
-charge_station( pose [ -49.000 -40.000 0 0 ] )
-charge_station( pose [ -49.000 -40.500 0 0 ] )
-charge_station( pose [ -49.000 -41.000 0 0 ] )
-charge_station( pose [ -49.000 -41.500 0 0 ] )
-charge_station( pose [ -49.000 -42.000 0 0 ] )
-charge_station( pose [ -49.000 -42.500 0 0 ] )
-charge_station( pose [ -49.000 -43.000 0 0 ] )
-charge_station( pose [ -49.000 -43.500 0 0 ] )
-charge_station( pose [ -49.000 -44.000 0 0 ] )
-charge_station( pose [ -49.000 -44.500 0 0 ] )
-charge_station( pose [ -49.000 -45.000 0 0 ] )
-charge_station( pose [ -49.000 -45.500 0 0 ] )
-charge_station( pose [ -49.000 -46.000 0 0 ] )
-charge_station( pose [ -49.000 -46.500 0 0 ] )
-charge_station( pose [ -49.000 -47.000 0 0 ] )
-charge_station( pose [ -49.000 -47.500 0 0 ] )
+#charge_station( pose [ -38.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -39.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -40.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -41.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -42.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -43.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -44.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -45.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -46.00 -49.9 0 90.000 ] )
+#charge_station( pose [ -47.0 -49.9 0 90.000 ] )
+charge_station( pose [ -49.9 -38.500 0 0 ] )
+charge_station( pose [ -49.9 -39.500 0 0 ] )
+charge_station( pose [ -49.9 -40.500 0 0 ] )
+charge_station( pose [ -49.9 -41.500 0 0 ] )
+charge_station( pose [ -49.9 -42.500 0 0 ] )
+charge_station( pose [ -49.9 -43.500 0 0 ] )
+charge_station( pose [ -49.9 -44.500 0 0 ] )
+charge_station( pose [ -49.9 -45.500 0 0 ] )
+charge_station( pose [ -49.9 -46.500 0 0 ] )
+charge_station( pose [ -49.9 -47.500 0 0 ] )
zone( color "orange"
pose [ -43.000 -43.000 0 0 ]
@@ -218,11 +190,11 @@
define buffer model
(
color "orange"
- size [ 0.900 13.000 1.000 ]
+ size [ 0.100 13.000 1.000 ]
)
-buffer( pose [ -49.500 -42.500 0 0 ] )
-buffer( pose [ -42.595 -49.527 0 90.000 ] )
+#buffer( pose [ -49.543 -42.500 0 0 ] )
+#buffer( pose [ -42.595 -49.527 0 90.000 ] )
define blinker model
(
@@ -260,27 +232,27 @@
define magentabot autorob( color "magenta" ctrl "fasr2 magenta" )
-redbot( pose [-11.388 12.267 0 166.581] kjoules 200 )
-redbot( pose [-12.208 11.744 0 135.003] kjoules 200 )
-redbot( pose [-12.289 10.814 0 147.497] kjoules 200 )
-redbot( pose [-10.642 11.563 0 123.708] kjoules 200 )
-redbot( pose [-11.378 11.274 0 143.281] kjoules 200 )
-redbot( pose [-10.200 11.012 0 46.529] kjoules 200 )
-redbot( pose [-13.006 13.061 0 168.778] kjoules 200 )
-redbot( pose [-12.466 13.333 0 163.730] kjoules 200 )
-redbot( pose [-13.740 12.740 0 157.546] kjoules 200 )
-redbot( pose [-15.434 11.856 0 138.616] kjoules 200 )
+redbot( pose [-11.388 12.267 0 166.581] kjoules 20 )
+redbot( pose [-12.208 11.744 0 135.003] kjoules 20 )
+redbot( pose [-12.289 10.814 0 147.497] kjoules 20 )
+redbot( pose [-10.642 11.563 0 123.708] kjoules 20 )
+redbot( pose [-11.378 11.274 0 143.281] kjoules 20 )
+redbot( pose [-10.200 11.012 0 46.529] kjoules 20 )
+redbot( pose [-13.006 13.061 0 168.778] kjoules 20 )
+redbot( pose [-12.466 13.333 0 163.730] kjoules 20 )
+redbot( pose [-13.740 12.740 0 157.546] kjoules 20 )
+redbot( pose [-15.434 11.856 0 138.616] kjoules 20 )
-redbot( pose [-12.841 10.666 0 125.831] kjoules 400 )
-redbot( pose [-12.710 12.157 0 125.831] kjoules 400 )
-redbot( pose [-12.214 12.593 0 125.831] kjoules 400 )
-redbot( pose [-11.461 13.123 0 125.831] kjoules 400 )
-redbot( pose [-10.679 13.036 0 125.831] kjoules 400 )
-redbot( pose [-12.277 14.027 0 125.831] kjoules 400 )
-redbot( pose [-13.981 13.377 0 125.831] kjoules 400 )
-redbot( pose [-13.372 12.243 0 125.831] kjoules 400 )
-redbot( pose [-13.296 11.331 0 125.831] kjoules 400 )
-redbot( pose [-13.944 11.986 0 125.831] kjoules 400 )
+redbot( pose [-12.841 10.666 0 125.831] kjoules 40 )
+redbot( pose [-12.710 12.157 0 125.831] kjoules 40 )
+redbot( pose [-12.214 12.593 0 125.831] kjoules 40 )
+redbot( pose [-11.461 13.123 0 125.831] kjoules 40 )
+redbot( pose [-10.679 13.036 0 125.831] kjoules 40 )
+redbot( pose [-12.277 14.027 0 125.831] kjoules 40 )
+redbot( pose [-13.981 13.377 0 125.831] kjoules 40 )
+redbot( pose [-13.372 12.243 0 125.831] kjoules 40 )
+redbot( pose [-13.296 11.331 0 125.831] kjoules 40 )
+redbot( pose [-13.944 11.986 0 125.831] kjoules 40 )
#redbot( pose [-14.580 14.279 0 125.831] kjoules 400 )
#redbot( pose [-12.922 13.866 0 125.831] kjoules 400 )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
