Revision: 8592
http://playerstage.svn.sourceforge.net/playerstage/?rev=8592&view=rev
Author: rtv
Date: 2010-03-18 01:31:15 +0000 (Thu, 18 Mar 2010)
Log Message:
-----------
broken queuing in fasr2
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/astar/findpath.cpp
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/examples/ctrl/fasr2.cc
code/stage/trunk/worlds/fasr2.world
Modified: code/stage/trunk/examples/ctrl/astar/findpath.cpp
===================================================================
--- code/stage/trunk/examples/ctrl/astar/findpath.cpp 2010-03-16 05:14:09 UTC
(rev 8591)
+++ code/stage/trunk/examples/ctrl/astar/findpath.cpp 2010-03-18 01:31:15 UTC
(rev 8592)
@@ -111,7 +111,8 @@
// AddSuccessor to give the successors to the AStar class. The A* specific
initialisation
// is done for each node internally, so here you just set the state
information that
// is specific to the application
-bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch,
MapSearchNode *parent_node )
+bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch,
+
MapSearchNode *parent_node )
{
int parent_x = -1;
@@ -123,12 +124,11 @@
parent_y = parent_node->y;
}
-
MapSearchNode NewNode;
// push each possible move except allowing the search to go backwards
- if( (GetMap( x-1, y ) < (unsigned int)9)
+ if( (GetMap( x-1, y ) < 9)
&& !((parent_x == x-1) && (parent_y == y))
)
{
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2010-03-16 05:14:09 UTC (rev
8591)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2010-03-18 01:31:15 UTC (rev
8592)
@@ -11,7 +11,7 @@
const double stopdist = 0.5;
const int avoidduration = 10;
const int workduration = 20;
-const int payload = 1;
+const unsigned int payload = 1;
double have[4][4] = {
// { -120, -180, 180, 180 }
Modified: code/stage/trunk/examples/ctrl/fasr2.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-16 05:14:09 UTC (rev
8591)
+++ code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-18 01:31:15 UTC (rev
8592)
@@ -11,8 +11,8 @@
const double avoidturn = 0.5;
const double minfrontdistance = 0.7;
const double stopdist = 0.5;
-const int avoidduration = 10;
-const int PAYLOAD = 1;
+const unsigned int avoidduration = 10;
+const unsigned int PAYLOAD = 1;
//const int TRAIL_LENGTH_MAX = 500;
@@ -25,8 +25,85 @@
-class Edge;
+// abstract base class
+class Queue
+{
+public:
+ Queue() {};
+ virtual ~Queue() {}
+
+ virtual int Clear() = 0;
+ virtual int Position( const char* id ) = 0;
+ virtual int Join( const char* id ) = 0;
+ virtual int Leave( const char* id ) = 0;
+};
+// on-host implementation
+#include <list>
+
+class LocalQueue : public Queue
+{
+public:
+ std::list<std::string> list;
+ //pthread_mutex_t* mutex;
+
+ LocalQueue() :
+ Queue(),
+ list()
+ //mutex( NULL )
+ {
+ //pthread_mutex_init( &mutex, NULL );
+ }
+
+ virtual int Clear()
+ {
+ list.clear();
+ return 0;
+ }
+
+ virtual int Position( const char* id )
+ {
+ std::list<std::string>::iterator it( std::find( list.begin(),
list.end(), std::string(id) ));
+
+ if( it == list.end() )
+ return -1;
+ else
+ return std::distance( list.begin(), it );
+ }
+
+ virtual int Join( const char* id )
+ {
+ list.push_back( id );
+ return 0;
+ }
+
+ virtual int Leave( const char* id )
+ {
+ list.erase( std::remove( list.begin(), list.end(), std::string(id)));
+ return 0;
+ }
+
+ virtual void Print( const char* prefix )
+ {
+ printf( "%s queue:\n", prefix );
+ FOR_EACH( it, list )
+ printf( "\t%s\n", it->c_str() );
+ }
+};
+
+
+class Node;
+
+class Edge
+{
+public:
+ Node* to;
+ double cost;
+
+ Edge( Node* to, double cost=1.0 )
+ : to(to), cost(cost) {}
+};
+
class Node
{
public:
@@ -37,7 +114,11 @@
Node( Pose pose )
: pose(pose), value(0), edges() {}
- ~Node();
+ ~Node()
+ {
+ FOR_EACH( it, edges )
+ { delete *it; }
+ }
void AddEdge( Edge* edge )
{
@@ -48,17 +129,7 @@
void Draw() const;
};
-class Edge
-{
-public:
- Node* to;
- double cost;
-
- Edge( Node* to, double cost=1.0 )
- : to(to), cost(cost) {}
-};
-
class Graph
{
public:
@@ -153,11 +224,6 @@
};
-Node::~Node()
-{
- FOR_EACH( it, edges )
- { delete *it; }
-}
void Node::Draw() const
{
@@ -166,9 +232,9 @@
//snprintf( buf, 32, "%.0f", value );
//Gl::draw_string( pose.x, pose.y+0.2, 0.1, buf );
- glBegin( GL_POINTS );
- glVertex2f( pose.x, pose.y );
- glEnd();
+ //glBegin( GL_POINTS );
+ //glVertex2f( pose.x, pose.y );
+ //glEnd();
glBegin( GL_LINES );
FOR_EACH( it, edges )
@@ -238,6 +304,8 @@
bool laser_sub;
bool ranger_sub;
+ static std::vector<LocalQueue> queues;
+
public:
Robot( ModelPosition* pos,
@@ -417,7 +485,7 @@
unsigned int dist = 0;
//FOR_EACH( it, path )
- for( std::vector<point_t>::reverse_iterator rit = path.rbegin();
+ for( std::vector<point_t>::const_reverse_iterator rit = path.rbegin();
rit != path.rend();
++rit )
{
@@ -802,10 +870,11 @@
stg_meters_t dest_dist = hypot( sourcepose.x-pose.x,
sourcepose.y-pose.y );
// if we're close, go get in line
- // if( dest_dist < sourcegeom.size.x )
- // mode = MODE_QUEUE;
+ if( dest_dist < sourcegeom.size.x )
+ JoinQueue(0);
+ else
+ LeaveQueue(0);
-
if( dest_dist < sourcegeom.size.x/2.0 &&
pos->GetFlagCount() < PAYLOAD )
{
@@ -871,13 +940,23 @@
}
- void Queue()
- {
-
+ void JoinQueue( unsigned int q )
+ {
+ //puts( "joing queue" );
+ if( queues[q].Position( pos->Token() ) < 0 )
+ {
+ queues[q].Join( pos->Token() );
+ queues[q].Print( "q0");
+ }
}
+ void LeaveQueue( unsigned int q )
+ {
+ queues[q].Leave( pos->Token() );
+ }
+
static int FlagIncr( Model* mod, Robot* robot )
@@ -901,6 +980,8 @@
uint8_t* Robot::map_data( NULL );
Model* Robot::map_model( NULL );
+std::vector<LocalQueue> Robot::queues(100);
+
void split( const std::string& text, const std::string& separators,
std::vector<std::string>& words)
{
int n = text.length();
@@ -918,6 +999,7 @@
const std::string sources[] = {"red", "green", "blue", "cyan", "yellow",
"magenta" };
const unsigned int sources_count = 6;
+
// Stage calls this when the model starts up
extern "C" int Init( Model* mod, CtrlArgs* args )
{
Modified: code/stage/trunk/worlds/fasr2.world
===================================================================
--- code/stage/trunk/worlds/fasr2.world 2010-03-16 05:14:09 UTC (rev 8591)
+++ code/stage/trunk/worlds/fasr2.world 2010-03-18 01:31:15 UTC (rev 8592)
@@ -23,9 +23,9 @@
(
size [ 1226.000 1080.000 ]
- center [ -36.643 -41.795 ]
+ center [ -0.805 -1.069 ]
rotate [ 0 0 ]
- scale 41.558
+ scale 9.771
show_data 1
show_flags 1
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
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
