Browse files

working on much more elaborate fasr2 demo

  • Loading branch information...
1 parent 6484adb commit 7e37217768346030f047fd1a78a54cbbcbbf31cd rtv committed Feb 19, 2010
View
BIN assets/blue.png
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
View
BIN assets/green.png
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
View
BIN assets/red.png
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
View
12 examples/ctrl/CMakeLists.txt
@@ -23,9 +23,18 @@ set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}
foreach( PLUGIN ${PLUGINS} )
ADD_LIBRARY( ${PLUGIN} MODULE ${PLUGIN}.cc )
+endforeach( PLUGIN )
+
+ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp )
+
+# add fasr2 to the list of plugins
+SET( PLUGINS ${PLUGINS} fasr2 )
+
+foreach( PLUGIN ${PLUGINS} )
TARGET_LINK_LIBRARIES( ${PLUGIN} stage )
endforeach( PLUGIN )
+
# delete the "lib" prefix from the plugin libraries
SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
@@ -37,6 +46,7 @@ SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
#endif( PLAYER_FOUND )
+
# install in <prefix>/lib
-install( TARGETS ${PLUGINS} DESTINATION lib)
+install( TARGETS ${PLUGINS} fasr2 DESTINATION lib)
View
16 examples/ctrl/astar/astar.h
@@ -0,0 +1,16 @@
+
+class point_t
+{
+public:
+ unsigned int x, y;
+
+ point_t( unsigned int x, unsigned int y ) : x(x), y(y) {}
+};
+
+bool astar( uint8_t* map,
+ unsigned int width,
+ unsigned int height,
+ const point_t start,
+ const point_t goal,
+ std::vector<point_t>& path );
+
View
350 examples/ctrl/astar/findpath.cpp
@@ -0,0 +1,350 @@
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// STL A* Search implementation
+// (C)2001 Justin Heyes-Jones
+//
+// Finding a path on a simple grid maze
+// This shows how to do shortest path finding using A*
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#include "stlastar.h" // See header for copyright and usage information
+
+#include <iostream>
+#include <math.h>
+
+#define DEBUG_LISTS 0
+#define DEBUG_LIST_LENGTHS_ONLY 0
+
+using namespace std;
+
+
+// map helper functions
+
+static uint8_t* _map = NULL;
+static unsigned int _map_width=0, _map_height=0;
+
+uint8_t GetMap( unsigned int x, unsigned int y )
+{
+ assert(_map);
+
+ if( x < 0 ||
+ x >= _map_width ||
+ y < 0 ||
+ y >= _map_height
+ )
+ {
+ return 9;
+ }
+
+ return _map[(y*_map_width)+x];
+}
+
+// Definitions
+
+class MapSearchNode
+{
+public:
+ unsigned int x; // the (x,y) positions of the node
+ unsigned int y;
+
+ MapSearchNode() { x = y = 0; }
+ MapSearchNode( unsigned int px, unsigned int py ) { x=px; y=py; }
+
+ float GoalDistanceEstimate( MapSearchNode &nodeGoal );
+ bool IsGoal( MapSearchNode &nodeGoal );
+ bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
+ float GetCost( MapSearchNode &successor );
+ bool IsSameState( MapSearchNode &rhs );
+
+ void PrintNodeInfo();
+
+
+};
+
+bool MapSearchNode::IsSameState( MapSearchNode &rhs )
+{
+
+ // same state in a maze search is simply when (x,y) are the same
+ if( (x == rhs.x) &&
+ (y == rhs.y) )
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+
+}
+
+void MapSearchNode::PrintNodeInfo()
+{
+ //cout << x << "\t" << y << endl;
+ cout << "Node position : (" << x << ", " << y << ")" << endl;
+}
+
+// Here's the heuristic function that estimates the distance from a Node
+// to the Goal.
+
+float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
+{
+ float xd = fabs(float(((float)x - (float)nodeGoal.x)));
+ float yd = fabs(float(((float)y - (float)nodeGoal.y)));
+ return xd + yd;
+ //return 1.001 * (xd + yd );
+}
+
+bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
+{
+
+ if( (x == nodeGoal.x) &&
+ (y == nodeGoal.y) )
+ {
+ return true;
+ }
+
+ return false;
+}
+
+// This generates the successors to the given Node. It uses a helper function called
+// 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 )
+{
+
+ int parent_x = -1;
+ int parent_y = -1;
+
+ if( parent_node )
+ {
+ parent_x = parent_node->x;
+ parent_y = parent_node->y;
+ }
+
+
+ MapSearchNode NewNode;
+
+ // push each possible move except allowing the search to go backwards
+
+ if( (GetMap( x-1, y ) < 9)
+ && !((parent_x == x-1) && (parent_y == y))
+ )
+ {
+ NewNode = MapSearchNode( x-1, y );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+ if( (GetMap( x, y-1 ) < 9)
+ && !((parent_x == x) && (parent_y == y-1))
+ )
+ {
+ NewNode = MapSearchNode( x, y-1 );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+ if( (GetMap( x+1, y ) < 9)
+ && !((parent_x == x+1) && (parent_y == y))
+ )
+ {
+ NewNode = MapSearchNode( x+1, y );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+
+ if( (GetMap( x, y+1 ) < 9)
+ && !((parent_x == x) && (parent_y == y+1))
+ )
+ {
+ NewNode = MapSearchNode( x, y+1 );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+ return true;
+}
+
+// given this node, what does it cost to move to successor. In the case
+// of our map the answer is the map terrain value at this node since that is
+// conceptually where we're moving
+
+float MapSearchNode::GetCost( MapSearchNode &successor )
+{
+ return (float) GetMap( x, y );
+
+}
+
+
+// Main
+
+#include "astar.h"
+
+bool astar( uint8_t* map,
+ unsigned int width,
+ unsigned int height,
+ const point_t start,
+ const point_t goal,
+ std::vector<point_t>& path )
+{
+ //cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";
+
+ // set the static vars
+ _map = map;
+ _map_width = width;
+ _map_height = height;
+
+ // Our sample problem defines the world as a 2d array representing a terrain
+ // Each element contains an integer from 0 to 5 which indicates the cost
+ // of travel across the terrain. Zero means the least possible difficulty
+ // in travelling (think ice rink if you can skate) whilst 5 represents the
+ // most difficult. 9 indicates that we cannot pass.
+
+ // Create an instance of the search class...
+
+ AStarSearch<MapSearchNode> astarsearch;
+
+ unsigned int SearchCount = 0;
+
+ const unsigned int NumSearches = 1;
+
+ bool path_found = false;
+
+ while(SearchCount < NumSearches)
+ {
+
+ // Create a start state
+ MapSearchNode nodeStart;
+ nodeStart.x = start.x;
+ nodeStart.y = start.y;
+
+ // Define the goal state
+ MapSearchNode nodeEnd;
+ nodeEnd.x = goal.x;
+ nodeEnd.y = goal.y;
+
+ // Set Start and goal states
+
+ astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
+
+ unsigned int SearchState;
+ unsigned int SearchSteps = 0;
+
+ do
+ {
+ SearchState = astarsearch.SearchStep();
+
+ SearchSteps++;
+
+ #if DEBUG_LISTS
+
+ cout << "Steps:" << SearchSteps << "\n";
+
+ int len = 0;
+
+ cout << "Open:\n";
+ MapSearchNode *p = astarsearch.GetOpenListStart();
+ while( p )
+ {
+ len++;
+ #if !DEBUG_LIST_LENGTHS_ONLY
+ ((MapSearchNode *)p)->PrintNodeInfo();
+ #endif
+ p = astarsearch.GetOpenListNext();
+
+ }
+
+ cout << "Open list has " << len << " nodes\n";
+
+ len = 0;
+
+ cout << "Closed:\n";
+ p = astarsearch.GetClosedListStart();
+ while( p )
+ {
+ len++;
+ #if !DEBUG_LIST_LENGTHS_ONLY
+ p->PrintNodeInfo();
+ #endif
+ p = astarsearch.GetClosedListNext();
+ }
+
+ cout << "Closed list has " << len << " nodes\n";
+ #endif
+
+ }
+ while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );
+
+ if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
+ {
+ //cout << "Search found goal state\n";
+
+ MapSearchNode *node = astarsearch.GetSolutionStart();
+
+#if DISPLAY_SOLUTION
+ cout << "Displaying solution\n";
+#endif
+ int steps = 0;
+
+ //node->PrintNodeInfo();
+
+ path.push_back( point_t( node->x, node->y ) );
+
+ for( ;; )
+ {
+ node = astarsearch.GetSolutionNext();
+
+ if( !node )
+ {
+ break;
+ }
+
+ //node->PrintNodeInfo();
+
+ path.push_back( point_t( node->x, node->y ) );
+
+ steps ++;
+
+ };
+
+ //cout << "Solution steps " << steps << endl;
+
+ // Once you're done with the solution you can free the nodes up
+ astarsearch.FreeSolutionNodes();
+
+ path_found = true;
+
+ }
+ else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED )
+ {
+ cout << "Search terminated. Did not find goal state\n";
+
+ }
+
+ // Display the number of loops the search went through
+ //cout << "SearchSteps : " << SearchSteps << "\n";
+
+ SearchCount ++;
+
+ astarsearch.EnsureMemoryFreed();
+ }
+
+ return path_found;
+}
+
+// // STL container iterator macros
+// #define VAR(V,init) __typeof(init) V=(init)
+// #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
+
+// int main( int argc, char *argv[] )
+// {
+// std::vector<point_t> path;
+
+// bool result = astar( map, 30, 30, point_t( 1,1 ), point_t( 25,20 ), path );
+
+// printf( "#%s:\n", result ? "PATH" : "NOPATH" );
+
+// FOR_EACH( it, path )
+// printf( "%d, %d\n", it->x, it->y );
+// puts("");
+// }
+
View
250 examples/ctrl/astar/fsa.h
@@ -0,0 +1,250 @@
+/*
+
+A* Algorithm Implementation using STL is
+Copyright (C)2001-2005 Justin Heyes-Jones
+
+Permission is given by the author to freely redistribute and
+include this code in any program as long as this credit is
+given where due.
+
+ COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED,
+ INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE
+ IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
+ OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND
+ PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED
+ CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL
+ DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY
+ NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF
+ WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE
+ OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
+ THIS DISCLAIMER.
+
+ Use at your own risk!
+
+
+
+ FixedSizeAllocator class
+ Copyright 2001 Justin Heyes-Jones
+
+ This class is a constant time O(1) memory manager for objects of
+ a specified type. The type is specified using a template class.
+
+ Memory is allocated from a fixed size buffer which you can specify in the
+ class constructor or use the default.
+
+ Using GetFirst and GetNext it is possible to iterate through the elements
+ one by one, and this would be the most common use for the class.
+
+ I would suggest using this class when you want O(1) add and delete
+ and you don't do much searching, which would be O(n). Structures such as binary
+ trees can be used instead to get O(logn) access time.
+
+*/
+
+#ifndef FSA_H
+#define FSA_H
+
+#include <string.h>
+#include <stdio.h>
+
+template <class USER_TYPE> class FixedSizeAllocator
+{
+
+public:
+ // Constants
+ enum
+ {
+ FSA_DEFAULT_SIZE = 100
+ };
+
+ // This class enables us to transparently manage the extra data
+ // needed to enable the user class to form part of the double-linked
+ // list class
+ struct FSA_ELEMENT
+ {
+ USER_TYPE UserType;
+
+ FSA_ELEMENT *pPrev;
+ FSA_ELEMENT *pNext;
+ };
+
+public: // methods
+ FixedSizeAllocator( unsigned int MaxElements = FSA_DEFAULT_SIZE ) :
+ m_MaxElements( MaxElements ),
+ m_pFirstUsed( NULL )
+ {
+ // Allocate enough memory for the maximum number of elements
+
+ m_pMemory = (FSA_ELEMENT *) (new char[ m_MaxElements * sizeof(FSA_ELEMENT) ]);
+
+ // Set the free list first pointer
+ m_pFirstFree = m_pMemory;
+
+ // Clear the memory
+ memset( m_pMemory, 0, sizeof( FSA_ELEMENT ) * m_MaxElements );
+
+ // Point at first element
+ FSA_ELEMENT *pElement = m_pFirstFree;
+
+ // Set the double linked free list
+ for( unsigned int i=0; i<m_MaxElements; i++ )
+ {
+ pElement->pPrev = pElement-1;
+ pElement->pNext = pElement+1;
+
+ pElement++;
+ }
+
+ // first element should have a null prev
+ m_pFirstFree->pPrev = NULL;
+ // last element should have a null next
+ (pElement-1)->pNext = NULL;
+
+ }
+
+
+ ~FixedSizeAllocator()
+ {
+ // Free up the memory
+ delete [] (char *) m_pMemory;
+ }
+
+ // Allocate a new USER_TYPE and return a pointer to it
+ USER_TYPE *alloc()
+ {
+
+ FSA_ELEMENT *pNewNode = NULL;
+
+ if( !m_pFirstFree )
+ {
+ return NULL;
+ }
+ else
+ {
+ pNewNode = m_pFirstFree;
+ m_pFirstFree = pNewNode->pNext;
+
+ // if the new node points to another free node then
+ // change that nodes prev free pointer...
+ if( pNewNode->pNext )
+ {
+ pNewNode->pNext->pPrev = NULL;
+ }
+
+ // node is now on the used list
+
+ pNewNode->pPrev = NULL; // the allocated node is always first in the list
+
+ if( m_pFirstUsed == NULL )
+ {
+ pNewNode->pNext = NULL; // no other nodes
+ }
+ else
+ {
+ m_pFirstUsed->pPrev = pNewNode; // insert this at the head of the used list
+ pNewNode->pNext = m_pFirstUsed;
+ }
+
+ m_pFirstUsed = pNewNode;
+ }
+
+ return reinterpret_cast<USER_TYPE*>(pNewNode);
+ }
+
+ // Free the given user type
+ // For efficiency I don't check whether the user_data is a valid
+ // pointer that was allocated. I may add some debug only checking
+ // (To add the debug check you'd need to make sure the pointer is in
+ // the m_pMemory area and is pointing at the start of a node)
+ void free( USER_TYPE *user_data )
+ {
+ FSA_ELEMENT *pNode = reinterpret_cast<FSA_ELEMENT*>(user_data);
+
+ // manage used list, remove this node from it
+ if( pNode->pPrev )
+ {
+ pNode->pPrev->pNext = pNode->pNext;
+ }
+ else
+ {
+ // this handles the case that we delete the first node in the used list
+ m_pFirstUsed = pNode->pNext;
+ }
+
+ if( pNode->pNext )
+ {
+ pNode->pNext->pPrev = pNode->pPrev;
+ }
+
+ // add to free list
+ if( m_pFirstFree == NULL )
+ {
+ // free list was empty
+ m_pFirstFree = pNode;
+ pNode->pPrev = NULL;
+ pNode->pNext = NULL;
+ }
+ else
+ {
+ // Add this node at the start of the free list
+ m_pFirstFree->pPrev = pNode;
+ pNode->pNext = m_pFirstFree;
+ m_pFirstFree = pNode;
+ }
+
+ }
+
+ // For debugging this displays both lists (using the prev/next list pointers)
+ void Debug()
+ {
+ printf( "free list " );
+
+ FSA_ELEMENT *p = m_pFirstFree;
+ while( p )
+ {
+ printf( "%x!%x ", p->pPrev, p->pNext );
+ p = p->pNext;
+ }
+ printf( "\n" );
+
+ printf( "used list " );
+
+ p = m_pFirstUsed;
+ while( p )
+ {
+ printf( "%x!%x ", p->pPrev, p->pNext );
+ p = p->pNext;
+ }
+ printf( "\n" );
+ }
+
+ // Iterators
+
+ USER_TYPE *GetFirst()
+ {
+ return reinterpret_cast<USER_TYPE *>(m_pFirstUsed);
+ }
+
+ USER_TYPE *GetNext( USER_TYPE *node )
+ {
+ return reinterpret_cast<USER_TYPE *>
+ (
+ (reinterpret_cast<FSA_ELEMENT *>(node))->pNext
+ );
+ }
+
+public: // data
+
+private: // methods
+
+private: // data
+
+ FSA_ELEMENT *m_pFirstFree;
+ FSA_ELEMENT *m_pFirstUsed;
+ unsigned int m_MaxElements;
+ FSA_ELEMENT *m_pMemory;
+
+};
+
+#endif // defined FSA_H
View
20 examples/ctrl/astar/license.txt
@@ -0,0 +1,20 @@
+Copyright (c) 2006 Justin Heyes-Jones
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+ the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included
+in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+
View
145 examples/ctrl/astar/readme.txt
@@ -0,0 +1,145 @@
+**********************************
+A* Algorithm by Justin Heyes-Jones
+**********************************
+
+***********
+Description
+***********
+
+This implementation is intended to be simple to read yet fairly
+efficient. To build it you can compile, with any recent C++ compiler,
+the following files :
+
+For 8-puzzle solver
+
+ 8puzzle.cpp
+ stlastar.h
+ optionally fsa.h
+
+Command line
+
+ 8puzzle with no arguments runs with one of the boards in the cpp file, you can
+ select the one you want changing the conditional compiliation instructions. Or if you
+ prefer pass in a board on the command line using digits for the tile positions, where
+ zero is the space. The board runs from left to right, each row at a time:
+
+ 8puzzle 013824765
+
+For path finder
+
+ findpath.cpp
+ stlastar.h
+ optionally fsa.h
+
+ pathfind has no arguments. You can edit the simple map in pathfind.cpp and the start
+ and goal co-ordinates to experiement with the pathfinder.
+
+Fixed size allocator notes: As mentioned briefly in the tutorial you can enable and disable the
+faster memory allocation. This allocates a fixed size block of memory, so you have to specify this size
+with the astar constructor. You need to enlarge it if you hit an out of memory assert during the
+search.
+
+Compilation notes:
+
+Microsoft Visual C++ : Confirmed working with version 8.0.50727 with some deprecation warnings
+I'm going to leave the deprecation warnings in so that it still works cleanly with GCC.
+TODO Make a non-deprecated compliant version using compiler checking
+
+GCC notes : Compiled using version 3.4.5 (MingW)
+
+Please let me know if it doesn't work for you and I will try to help. I cannot help if you are using
+an old compiler such as Turbo C++, since I update the code to meet Ansi Standard C++ as required.
+
+At least in as far as the Microsoft and GCC compilers adhere to Ansi and add breaking changes.
+
+History:
+
+Updated 1th February 2009
+**********************************
+
+Fixed Manhattan distance bug. Should use absolute values.
+
+Got rid of sprintfs, use cout instead.
+
+Updated 3rd August 2006
+**********************************
+
+Fixed memory leak
+Fixed special case handling for finding better path to a closed node
+Fixed bug with comparing the start node with a new node by pointer instead of value
+Changed code to use Manhattan distance heuristic with pathfind.cpp as it is more correct
+
+
+Updated 27th September 2005
+**********************************
+
+Thanks to Gyan singh for pointing out the code no longer compiles under GCC.
+
+Well, that was the case. I've removed a Gnu offending conio.h include, and added lots more typename
+keywords, which seem to be required now when making an iterator using a template type.
+
+If anyone knows what the breaking change to the compiler was for, let me know.
+
+Updated 6th September 2005
+**********************************
+
+ Finally set the path to fsa.h correctly, sorry about that.
+ 8puzzle.cpp now defaults to using a demo puzzle that solves in a short time.
+ Added typename keyword to comply with latest ISO standards.
+
+Updated November 26th 2001
+**********************************
+
+ Fixed a bug. When a node is deleted from the Open list I did sort_heap when make_heap
+ is needed to keep the heap structure valid. This causes the search to go awry under some
+ conditions. Thanks to Mike Ryynanen for tracking this down.
+
+justinhj@hotmail.com
+
+http://www.geocities.com/jheyesjones/astar.html
+
+or
+
+http://www.heyes-jones.com/astar.html
+
+A* Algorithm Implementation using STL is
+Copyright (C)2001-2005 Justin Heyes-Jones
+
+Permission is given by the author to freely redistribute and
+include this code in any program as long as this credit is
+given where due.
+
+ COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED,
+ INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE
+ IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
+ OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND
+ PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED
+ CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL
+ DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY
+ NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF
+ WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE
+ OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
+ THIS DISCLAIMER.
+
+ Use at your own risk!
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
View
754 examples/ctrl/astar/stlastar.h
@@ -0,0 +1,754 @@
+/*
+A* Algorithm Implementation using STL is
+Copyright (C)2001-2005 Justin Heyes-Jones
+
+Permission is given by the author to freely redistribute and
+include this code in any program as long as this credit is
+given where due.
+
+ COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED,
+ INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE
+ IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
+ OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND
+ PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED
+ CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL
+ DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY
+ NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF
+ WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE
+ OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
+ THIS DISCLAIMER.
+
+ Use at your own risk!
+
+*/
+
+// used for text debugging
+#include <iostream>
+#include <stdio.h>
+//#include <conio.h>
+#include <assert.h>
+
+// stl includes
+#include <algorithm>
+#include <set>
+#include <vector>
+
+using namespace std;
+
+// fast fixed size memory allocator, used for fast node memory management
+#include "fsa.h"
+
+// Fixed size memory allocator can be disabled to compare performance
+// Uses std new and delete instead if you turn it off
+#define USE_FSA_MEMORY 1
+
+// disable warning that debugging information has lines that are truncated
+// occurs in stl headers
+#pragma warning( disable : 4786 )
+
+// The AStar search class. UserState is the users state space type
+template <class UserState> class AStarSearch
+{
+
+public: // data
+
+ enum
+ {
+ SEARCH_STATE_NOT_INITIALISED,
+ SEARCH_STATE_SEARCHING,
+ SEARCH_STATE_SUCCEEDED,
+ SEARCH_STATE_FAILED,
+ SEARCH_STATE_OUT_OF_MEMORY,
+ SEARCH_STATE_INVALID
+ };
+
+
+ // A node represents a possible state in the search
+ // The user provided state type is included inside this type
+
+ public:
+
+ class Node
+ {
+ public:
+
+ Node *parent; // used during the search to record the parent of successor nodes
+ Node *child; // used after the search for the application to view the search in reverse
+
+ float g; // cost of this node + it's predecessors
+ float h; // heuristic estimate of distance to goal
+ float f; // sum of cumulative cost of predecessors and self and heuristic
+
+ Node() :
+ parent( 0 ),
+ child( 0 ),
+ g( 0.0f ),
+ h( 0.0f ),
+ f( 0.0f )
+ {
+ }
+
+ UserState m_UserState;
+ };
+
+
+ // For sorting the heap the STL needs compare function that lets us compare
+ // the f value of two nodes
+
+ class HeapCompare_f
+ {
+ public:
+
+ bool operator() ( const Node *x, const Node *y ) const
+ {
+ return x->f > y->f;
+ }
+ };
+
+
+public: // methods
+
+
+ // constructor just initialises private data
+ AStarSearch( int MaxNodes = 1000 ) :
+ m_AllocateNodeCount(0),
+#if USE_FSA_MEMORY
+ m_FixedSizeAllocator( MaxNodes ),
+#endif
+ m_State( SEARCH_STATE_NOT_INITIALISED ),
+ m_CurrentSolutionNode( NULL ),
+ m_CancelRequest( false )
+ {
+ }
+
+ // call at any time to cancel the search and free up all the memory
+ void CancelSearch()
+ {
+ m_CancelRequest = true;
+ }
+
+ // Set Start and goal states
+ void SetStartAndGoalStates( UserState &Start, UserState &Goal )
+ {
+ m_CancelRequest = false;
+
+ m_Start = AllocateNode();
+ m_Goal = AllocateNode();
+
+ assert((m_Start != NULL && m_Goal != NULL));
+
+ m_Start->m_UserState = Start;
+ m_Goal->m_UserState = Goal;
+
+ m_State = SEARCH_STATE_SEARCHING;
+
+ // Initialise the AStar specific parts of the Start Node
+ // The user only needs fill out the state information
+
+ m_Start->g = 0;
+ m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
+ m_Start->f = m_Start->g + m_Start->h;
+ m_Start->parent = 0;
+
+ // Push the start node on the Open list
+
+ m_OpenList.push_back( m_Start ); // heap now unsorted
+
+ // Sort back element into heap
+ push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+
+ // Initialise counter for search steps
+ m_Steps = 0;
+ }
+
+ // Advances search one step
+ unsigned int SearchStep()
+ {
+ // Firstly break if the user has not initialised the search
+ assert( (m_State > SEARCH_STATE_NOT_INITIALISED) &&
+ (m_State < SEARCH_STATE_INVALID) );
+
+ // Next I want it to be safe to do a searchstep once the search has succeeded...
+ if( (m_State == SEARCH_STATE_SUCCEEDED) ||
+ (m_State == SEARCH_STATE_FAILED)
+ )
+ {
+ return m_State;
+ }
+
+ // Failure is defined as emptying the open list as there is nothing left to
+ // search...
+ // New: Allow user abort
+ if( m_OpenList.empty() || m_CancelRequest )
+ {
+ FreeAllNodes();
+ m_State = SEARCH_STATE_FAILED;
+ return m_State;
+ }
+
+ // Incremement step count
+ m_Steps ++;
+
+ // Pop the best node (the one with the lowest f)
+ Node *n = m_OpenList.front(); // get pointer to the node
+ pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+ m_OpenList.pop_back();
+
+ // Check for the goal, once we pop that we're done
+ if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
+ {
+ // The user is going to use the Goal Node he passed in
+ // so copy the parent pointer of n
+ m_Goal->parent = n->parent;
+
+ // A special case is that the goal was passed in as the start state
+ // so handle that here
+ if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) )
+ {
+ FreeNode( n );
+
+ // set the child pointers in each node (except Goal which has no child)
+ Node *nodeChild = m_Goal;
+ Node *nodeParent = m_Goal->parent;
+
+ do
+ {
+ nodeParent->child = nodeChild;
+
+ nodeChild = nodeParent;
+ nodeParent = nodeParent->parent;
+
+ }
+ while( nodeChild != m_Start ); // Start is always the first node by definition
+
+ }
+
+ // delete nodes that aren't needed for the solution
+ FreeUnusedNodes();
+
+ m_State = SEARCH_STATE_SUCCEEDED;
+
+ return m_State;
+ }
+ else // not goal
+ {
+
+ // We now need to generate the successors of this node
+ // The user helps us to do this, and we keep the new nodes in
+ // m_Successors ...
+
+ m_Successors.clear(); // empty vector of successor nodes to n
+
+ // User provides this functions and uses AddSuccessor to add each successor of
+ // node 'n' to m_Successors
+ bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL );
+
+ if( !ret )
+ {
+
+ typename vector< Node * >::iterator successor;
+
+ // free the nodes that may previously have been added
+ for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
+ {
+ FreeNode( (*successor) );
+ }
+
+ m_Successors.clear(); // empty vector of successor nodes to n
+
+ // free up everything else we allocated
+ FreeAllNodes();
+
+ m_State = SEARCH_STATE_OUT_OF_MEMORY;
+ return m_State;
+ }
+
+ // Now handle each successor to the current node ...
+ for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
+ {
+
+ // The g value for this successor ...
+ float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState );
+
+ // Now we need to find whether the node is on the open or closed lists
+ // If it is but the node that is already on them is better (lower g)
+ // then we can forget about this successor
+
+ // First linear search of open list to find node
+
+ typename vector< Node * >::iterator openlist_result;
+
+ for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ )
+ {
+ if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
+ {
+ break;
+ }
+ }
+
+ if( openlist_result != m_OpenList.end() )
+ {
+
+ // we found this state on open
+
+ if( (*openlist_result)->g <= newg )
+ {
+ FreeNode( (*successor) );
+
+ // the one on Open is cheaper than this one
+ continue;
+ }
+ }
+
+ typename vector< Node * >::iterator closedlist_result;
+
+ for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ )
+ {
+ if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
+ {
+ break;
+ }
+ }
+
+ if( closedlist_result != m_ClosedList.end() )
+ {
+
+ // we found this state on closed
+
+ if( (*closedlist_result)->g <= newg )
+ {
+ // the one on Closed is cheaper than this one
+ FreeNode( (*successor) );
+
+ continue;
+ }
+ }
+
+ // This node is the best node so far with this particular state
+ // so lets keep it and set up its AStar specific data ...
+
+ (*successor)->parent = n;
+ (*successor)->g = newg;
+ (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
+ (*successor)->f = (*successor)->g + (*successor)->h;
+
+ // Remove successor from closed if it was on it
+
+ if( closedlist_result != m_ClosedList.end() )
+ {
+ // remove it from Closed
+ FreeNode( (*closedlist_result) );
+ m_ClosedList.erase( closedlist_result );
+
+ // Fix thanks to ...
+ // Greg Douglas <gregdouglasmail@gmail.com>
+ // who noticed that this code path was incorrect
+ // Here we have found a new state which is already CLOSED
+ // anus
+
+ }
+
+ // Update old version of this node
+ if( openlist_result != m_OpenList.end() )
+ {
+
+ FreeNode( (*openlist_result) );
+ m_OpenList.erase( openlist_result );
+
+ // re-make the heap
+ // make_heap rather than sort_heap is an essential bug fix
+ // thanks to Mike Ryynanen for pointing this out and then explaining
+ // it in detail. sort_heap called on an invalid heap does not work
+ make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+
+ }
+
+ // heap now unsorted
+ m_OpenList.push_back( (*successor) );
+
+ // sort back element into heap
+ push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+
+ }
+
+ // push n onto Closed, as we have expanded it now
+
+ m_ClosedList.push_back( n );
+
+ } // end else (not goal so expand)
+
+ return m_State; // Succeeded bool is false at this point.
+
+ }
+
+ // User calls this to add a successor to a list of successors
+ // when expanding the search frontier
+ bool AddSuccessor( UserState &State )
+ {
+ Node *node = AllocateNode();
+
+ if( node )
+ {
+ node->m_UserState = State;
+
+ m_Successors.push_back( node );
+
+ return true;
+ }
+
+ return false;
+ }
+
+ // Free the solution nodes
+ // This is done to clean up all used Node memory when you are done with the
+ // search
+ void FreeSolutionNodes()
+ {
+ Node *n = m_Start;
+
+ if( m_Start->child )
+ {
+ do
+ {
+ Node *del = n;
+ n = n->child;
+ FreeNode( del );
+
+ del = NULL;
+
+ } while( n != m_Goal );
+
+ FreeNode( n ); // Delete the goal
+
+ }
+ else
+ {
+ // if the start node is the solution we need to just delete the start and goal
+ // nodes
+ FreeNode( m_Start );
+ FreeNode( m_Goal );
+ }
+
+ }
+
+ // Functions for traversing the solution
+
+ // Get start node
+ UserState *GetSolutionStart()
+ {
+ m_CurrentSolutionNode = m_Start;
+ if( m_Start )
+ {
+ return &m_Start->m_UserState;
+ }
+ else
+ {
+ return NULL;
+ }
+ }
+
+ // Get next node
+ UserState *GetSolutionNext()
+ {
+ if( m_CurrentSolutionNode )
+ {
+ if( m_CurrentSolutionNode->child )
+ {
+
+ Node *child = m_CurrentSolutionNode->child;
+
+ m_CurrentSolutionNode = m_CurrentSolutionNode->child;
+
+ return &child->m_UserState;
+ }
+ }
+
+ return NULL;
+ }
+
+ // Get end node
+ UserState *GetSolutionEnd()
+ {
+ m_CurrentSolutionNode = m_Goal;
+ if( m_Goal )
+ {
+ return &m_Goal->m_UserState;
+ }
+ else
+ {
+ return NULL;
+ }
+ }
+
+ // Step solution iterator backwards
+ UserState *GetSolutionPrev()
+ {
+ if( m_CurrentSolutionNode )
+ {
+ if( m_CurrentSolutionNode->parent )
+ {
+
+ Node *parent = m_CurrentSolutionNode->parent;
+
+ m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
+
+ return &parent->m_UserState;
+ }
+ }
+
+ return NULL;
+ }
+
+ // For educational use and debugging it is useful to be able to view
+ // the open and closed list at each step, here are two functions to allow that.
+
+ UserState *GetOpenListStart()
+ {
+ float f,g,h;
+ return GetOpenListStart( f,g,h );
+ }
+
+ UserState *GetOpenListStart( float &f, float &g, float &h )
+ {
+ iterDbgOpen = m_OpenList.begin();
+ if( iterDbgOpen != m_OpenList.end() )
+ {
+ f = (*iterDbgOpen)->f;
+ g = (*iterDbgOpen)->g;
+ h = (*iterDbgOpen)->h;
+ return &(*iterDbgOpen)->m_UserState;
+ }
+
+ return NULL;
+ }
+
+ UserState *GetOpenListNext()
+ {
+ float f,g,h;
+ return GetOpenListNext( f,g,h );
+ }
+
+ UserState *GetOpenListNext( float &f, float &g, float &h )
+ {
+ iterDbgOpen++;
+ if( iterDbgOpen != m_OpenList.end() )
+ {
+ f = (*iterDbgOpen)->f;
+ g = (*iterDbgOpen)->g;
+ h = (*iterDbgOpen)->h;
+ return &(*iterDbgOpen)->m_UserState;
+ }
+
+ return NULL;
+ }
+
+ UserState *GetClosedListStart()
+ {
+ float f,g,h;
+ return GetClosedListStart( f,g,h );
+ }
+
+ UserState *GetClosedListStart( float &f, float &g, float &h )
+ {
+ iterDbgClosed = m_ClosedList.begin();
+ if( iterDbgClosed != m_ClosedList.end() )
+ {
+ f = (*iterDbgClosed)->f;
+ g = (*iterDbgClosed)->g;
+ h = (*iterDbgClosed)->h;
+
+ return &(*iterDbgClosed)->m_UserState;
+ }
+
+ return NULL;
+ }
+
+ UserState *GetClosedListNext()
+ {
+ float f,g,h;
+ return GetClosedListNext( f,g,h );
+ }
+
+ UserState *GetClosedListNext( float &f, float &g, float &h )
+ {
+ iterDbgClosed++;
+ if( iterDbgClosed != m_ClosedList.end() )
+ {
+ f = (*iterDbgClosed)->f;
+ g = (*iterDbgClosed)->g;
+ h = (*iterDbgClosed)->h;
+
+ return &(*iterDbgClosed)->m_UserState;
+ }
+
+ return NULL;
+ }
+
+ // Get the number of steps
+
+ int GetStepCount() { return m_Steps; }
+
+ void EnsureMemoryFreed()
+ {
+#if USE_FSA_MEMORY
+ assert(m_AllocateNodeCount == 0);
+#endif
+
+ }
+
+private: // methods
+
+ // This is called when a search fails or is cancelled to free all used
+ // memory
+ void FreeAllNodes()
+ {
+ // iterate open list and delete all nodes
+ typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
+
+ while( iterOpen != m_OpenList.end() )
+ {
+ Node *n = (*iterOpen);
+ FreeNode( n );
+
+ iterOpen ++;
+ }
+
+ m_OpenList.clear();
+
+ // iterate closed list and delete unused nodes
+ typename vector< Node * >::iterator iterClosed;
+
+ for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
+ {
+ Node *n = (*iterClosed);
+ FreeNode( n );
+ }
+
+ m_ClosedList.clear();
+
+ // delete the goal
+
+ FreeNode(m_Goal);
+ }
+
+
+ // This call is made by the search class when the search ends. A lot of nodes may be
+ // created that are still present when the search ends. They will be deleted by this
+ // routine once the search ends
+ void FreeUnusedNodes()
+ {
+ // iterate open list and delete unused nodes
+ typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
+
+ while( iterOpen != m_OpenList.end() )
+ {
+ Node *n = (*iterOpen);
+
+ if( !n->child )
+ {
+ FreeNode( n );
+
+ n = NULL;
+ }
+
+ iterOpen ++;
+ }
+
+ m_OpenList.clear();
+
+ // iterate closed list and delete unused nodes
+ typename vector< Node * >::iterator iterClosed;
+
+ for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
+ {
+ Node *n = (*iterClosed);
+
+ if( !n->child )
+ {
+ FreeNode( n );
+ n = NULL;
+
+ }
+ }
+
+ m_ClosedList.clear();
+
+ }
+
+ // Node memory management
+ Node *AllocateNode()
+ {
+
+#if !USE_FSA_MEMORY
+ Node *p = new Node;
+ return p;
+#else
+ Node *address = m_FixedSizeAllocator.alloc();
+
+ if( !address )
+ {
+ return NULL;
+ }
+ m_AllocateNodeCount ++;
+ Node *p = new (address) Node;
+ return p;
+#endif
+ }
+
+ void FreeNode( Node *node )
+ {
+
+ m_AllocateNodeCount --;
+
+#if !USE_FSA_MEMORY
+ delete node;
+#else
+ m_FixedSizeAllocator.free( node );
+#endif
+ }
+
+private: // data
+
+ // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article)
+ vector< Node *> m_OpenList;
+
+ // Closed list is a vector.
+ vector< Node * > m_ClosedList;
+
+ // Successors is a vector filled out by the user each type successors to a node
+ // are generated
+ vector< Node * > m_Successors;
+
+ // State
+ unsigned int m_State;
+
+ // Counts steps
+ int m_Steps;
+
+ // Start and goal state pointers
+ Node *m_Start;
+ Node *m_Goal;
+
+ Node *m_CurrentSolutionNode;
+
+#if USE_FSA_MEMORY
+ // Memory
+ FixedSizeAllocator<Node> m_FixedSizeAllocator;
+#endif
+
+ //Debug : need to keep these two iterators around
+ // for the user Dbg functions
+ typename vector< Node * >::iterator iterDbgOpen;
+ typename vector< Node * >::iterator iterDbgClosed;
+
+ // debugging : count memory allocation and free's
+ int m_AllocateNodeCount;
+
+ bool m_CancelRequest;
+
+};
+
+
+
+
View
742 examples/ctrl/fasr2.cc
@@ -0,0 +1,742 @@
+#include "stage.hh"
+using namespace Stg;
+
+#include "astar/astar.h"
+
+const bool verbose = false;
+
+// navigation control params
+const double cruisespeed = 0.4;
+const double avoidspeed = 0.05;
+const double avoidturn = 0.5;
+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;
+
+double have[4][4] = {
+ // { -120, -180, 180, 180 }
+ //{ -90, -120, 180, 90 },
+ { 90, 180, 180, 180 },
+ { 90, -90, 180, 90 },
+ { 90, 90, 180, 90 },
+ { 0, 45, 0, 0}
+};
+
+double need[4][4] = {
+ { -120, -180, 180, 180 },
+ { -90, -120, 180, 90 },
+ { -90, -90, 180, 180 },
+ { -90, -180, -90, -90 }
+};
+
+double refuel[4][4] = {
+ { 0, 0, 45, 120 },
+ { 0,-90, -60, -160 },
+ { -90, -90, 180, 180 },
+ { -90, -180, -90, -90 }
+};
+
+typedef enum {
+ MODE_WORK=0,
+ MODE_DOCK,
+ MODE_UNDOCK
+} nav_mode_t;
+
+
+
+
+class Edge;
+
+class Node
+{
+public:
+ Pose pose;
+ double value;
+ std::vector<Edge*> edges;
+
+ Node( const Pose& pose )
+ : pose(pose), value(0), edges() {}
+
+ ~Node();
+
+ void AddEdge( Edge* edge )
+ {
+ assert(edge);
+ edges.push_back( edge );
+ }
+
+ void Draw() const;
+};
+
+class Edge
+{
+public:
+ Node* to;
+ double cost;
+
+ Edge( Node* to, double cost=1.0 )
+ : to(to), cost(cost) {}
+};
+
+class Graph
+{
+public:
+ std::vector<Node*> nodes;
+
+ Graph(){}
+ ~Graph() { FOR_EACH( it, nodes ){ delete *it; }}
+
+ void AddNode( Node* node ){ nodes.push_back( node ); }
+
+ void PopFront()
+ {
+ const std::vector<Node*>::iterator& it = nodes.begin();
+ delete *it;
+ nodes.erase( it );
+ }
+
+ void Draw() const
+ {
+ glPointSize(4);
+ FOR_EACH( it, nodes ){ (*it)->Draw(); }
+ }
+};
+
+
+class GraphVis : public Visualizer
+{
+public:
+ Graph& graph;
+
+ GraphVis( Graph& graph )
+ : Visualizer( "graph", "vis_graph" ), graph(graph) {}
+ virtual ~GraphVis(){}
+
+ virtual void Visualize( Model* mod, Camera* cam )
+ {
+
+ glPushMatrix();
+
+ Gl::pose_inverse_shift( mod->GetGlobalPose() );
+
+ //mod->PushColor( 1,0,0,1 );
+
+ mod->PushColor( mod->GetColor() );
+ graph.Draw();
+ mod->PopColor();
+
+ glPopMatrix();
+ }
+};
+
+
+Node::~Node()
+{
+ FOR_EACH( it, edges )
+ { delete *it; }
+}
+
+void Node::Draw() const
+{
+ // print value
+ //char buf[32];
+ //snprintf( buf, 32, "%.2f", value );
+ //Gl::draw_string( pose.x, pose.y+0.2, 0.1, buf );
+
+ glBegin( GL_POINTS );
+ glVertex2f( pose.x, pose.y );
+ glEnd();
+
+ glBegin( GL_LINES );
+ FOR_EACH( it, edges )
+ {
+ glVertex2f( pose.x, pose.y );
+ glVertex2f( (*it)->to->pose.x, (*it)->to->pose.y );
+ }
+ glEnd();
+}
+
+unsigned int MetersToCell( stg_meters_t m, stg_meters_t size_m, unsigned int size_c )
+{
+ m += size_m / 2.0; // shift to local coords
+ m /= size_m / (float)size_c; // scale
+ return (unsigned int)floor(m); // quantize
+}
+
+stg_meters_t CellToMeters( unsigned int c, stg_meters_t size_m, unsigned int size_c )
+{
+ stg_meters_t cell_size = size_m/(float)size_c;
+ stg_meters_t m = c * cell_size; // scale
+ m -= size_m / 2.0; // shift to local coords
+
+
+ return m + cell_size/2.0; // offset to cell center
+}
+
+
+class Robot
+{
+private:
+ ModelPosition* pos;
+ ModelLaser* laser;
+ ModelRanger* ranger;
+ ModelFiducial* fiducial;
+ Model *source, *sink;
+ int avoidcount, randcount;
+ int work_get, work_put;
+ bool charger_ahoy;
+ double charger_bearing;
+ double charger_range;
+ double charger_heading;
+ nav_mode_t mode;
+ bool at_dest;
+
+ Graph graph;
+ GraphVis graphvis;
+ Node* last_node;
+ unsigned int node_interval;
+ unsigned int node_interval_countdown;
+
+ static unsigned int map_width;
+ static unsigned int map_height;
+ static uint8_t* map_data;
+
+public:
+ Robot( ModelPosition* pos,
+ Model* source,
+ Model* sink )
+ : pos(pos),
+ laser( (ModelLaser*)pos->GetUnusedModelOfType( "laser" )),
+ ranger( (ModelRanger*)pos->GetUnusedModelOfType( "ranger" )),
+ fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( "fiducial" )),
+ source(source),
+ sink(sink),
+ avoidcount(0),
+ randcount(0),
+ work_get(0),
+ work_put(0),
+ charger_ahoy(false),
+ charger_bearing(0),
+ charger_range(0),
+ charger_heading(0),
+ mode(MODE_WORK),
+ at_dest( false ),
+ graph(),
+ graphvis( graph ),
+ last_node( NULL ),
+ node_interval( 20 ),
+ node_interval_countdown( node_interval )
+ {
+ // need at least these models to get any work done
+ // (pos must be good, as we used it in the initialization list)
+ assert( laser );
+ assert( source );
+ assert( sink );
+
+ // pos->GetUnusedModelOfType( "laser" );
+
+ // PositionUpdate() checks to see if we reached source or sink
+ pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this );
+ pos->Subscribe();
+
+ // LaserUpdate() controls the robot, by reading from laser and
+ // writing to position
+ laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
+ laser->Subscribe();
+
+ fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this );
+ fiducial->Subscribe();
+
+ //pos->AddFlagIncrCallback( (stg_model_callback_t)FlagIncr, NULL );
+ //pos->AddFlagDecrCallback( (stg_model_callback_t)FlagDecr, NULL );
+
+ pos->AddVisualizer( &graphvis, true );
+
+ // get the map
+ Model* cave = pos->GetWorld()->GetModel( "cave" );
+ assert(cave);
+ Geom g = cave->GetGeom();
+
+
+ if( map_data == NULL )
+ {
+ map_data = new uint8_t[map_width*map_height*2];
+
+ // MUST clear the data, since Model::Rasterize() only enters
+ // non-zero pixels
+ memset( map_data, 0, sizeof(uint8_t) * map_width * map_height);
+
+ cave->Rasterize( map_data,
+ map_width,
+ map_height,
+ g.size.x/(float)map_width,
+ g.size.y/(float)map_height );
+
+ // putchar( '\n' );
+ // for( unsigned int y=0; y<map_height; y++ )
+ // {
+ // for( unsigned int x=0; x<map_width; x++ )
+ // printf( "%3d,", map_data[x + ((map_height-y-1)*map_width)] ? 999 : 1 );
+
+ // puts("");
+ // }
+
+ // fix the node costs for astar: 0=>1, 1=>9
+
+ unsigned int sz = map_width * map_height;
+ for( unsigned int i=0; i<sz; i++ )
+ {
+ if( map_data[i] == 0 )
+ map_data[i] = 1;
+ else if( map_data[i] == 1 )
+ map_data[i] = 9;
+ else
+ printf( "bad value %d in map at index %d\n", (int)map_data[i], (int)i );
+
+ assert( (map_data[i] == 1) || (map_data[i] == 9) );
+ }
+ }
+
+ Pose pose = pos->GetPose();
+ Pose sp = source->GetPose();
+
+ point_t start( MetersToCell(pose.x, g.size.x, map_width),
+ MetersToCell(pose.y, g.size.y, map_height) );
+ point_t goal( MetersToCell(sp.x, g.size.x, map_width),
+ MetersToCell(sp.y, g.size.y, map_height) );
+
+ //printf( "searching from (%.2f, %.2f) [%d, %d]\n", pose.x, pose.y, start.x, start.y );
+ //printf( "searching to (%.2f, %.2f) [%d, %d]\n", sp.x, sp.y, goal.x, goal.y );
+
+ std::vector<point_t> path;
+ bool result = astar( map_data,
+ map_width,
+ map_height,
+ start,
+ goal,
+ path );
+
+ //printf( "#%s:\n", result ? "PATH" : "NOPATH" );
+
+ FOR_EACH( it, path )
+ {
+ //printf( "%d, %d\n", it->x, it->y );
+
+ Node* node = new Node( Pose( CellToMeters(it->x, g.size.x, map_width ),
+ CellToMeters(it->y, g.size.y, map_height),
+ 0, 0 ) );
+ graph.AddNode( node );
+
+ if( last_node )
+ last_node->AddEdge( new Edge( node ) );
+
+ last_node = node;
+ }
+
+ //puts("");
+ }
+
+ void Dock()
+ {
+ if( charger_ahoy )
+ {
+ double a_goal = normalize( charger_bearing );
+
+ // if( pos->Stalled() )
+ // {
+ // puts( "stalled. stopping" );
+ // pos->Stop();
+ // }
+ // else
+
+ if( charger_range > 0.5 )
+ {
+ if( !ObstacleAvoid() )
+ {
+ pos->SetXSpeed( cruisespeed );
+ pos->SetTurnSpeed( a_goal );
+ }
+ }
+ else
+ {
+ pos->SetTurnSpeed( a_goal );
+ pos->SetXSpeed( 0.02 ); // creep towards it
+
+ if( charger_range < 0.08 ) // close enough
+ pos->Stop();
+
+ if( pos->Stalled() ) // touching
+ pos->SetXSpeed( -0.01 ); // back off a bit
+
+ }
+ }
+ else
+ {
+ //printf( "docking but can't see a charger\n" );
+ pos->Stop();
+ mode = MODE_WORK; // should get us back on track eventually
+ }
+
+ // if the battery is charged, go back to work
+ if( Full() )
+ {
+ //printf( "fully charged, now back to work\n" );
+ mode = MODE_UNDOCK;
+ }
+ }
+
+
+ void UnDock()
+ {
+ const stg_meters_t back_off_distance = 0.3;
+ const stg_meters_t back_off_speed = -0.05;
+
+ // back up a bit
+ if( charger_range < back_off_distance )
+ pos->SetXSpeed( back_off_speed );
+ else
+ pos->SetXSpeed( 0.0 );
+
+ if( charger_range > back_off_distance )
+ mode = MODE_WORK;
+ }
+
+ bool ObstacleAvoid()
+ {
+ 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;
+
+ // Get the data
+ uint32_t sample_count=0;
+ ModelLaser::Sample* scan = laser->GetSamples( &sample_count );
+
+ for (uint32_t i = 0; i < sample_count; i++)
+ {
+ if( verbose ) printf( "%.3f ", scan[i].range );
+
+ if( (i > (sample_count/4))
+ && (i < (sample_count - (sample_count/4)))
+ && 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 = std::min( minleft, scan[i].range );
+ else
+ minright = std::min( minright, scan[i].range );
+ }
+
+ if( verbose )
+ {
+ puts( "" );
+ printf( "minleft %.3f \n", minleft );
+ printf( "minright %.3f\n ", minright );
+ }
+
+ if( obstruction || stop || (avoidcount>0) )
+ {
+ if( verbose ) printf( "Avoid %d\n", avoidcount );
+
+ pos->SetXSpeed( stop ? 0.0 : avoidspeed );
+
+ /* once we start avoiding, select a turn direction and stick
+ with it for a few iterations */
+ if( avoidcount < 1 )
+ {
+ if( verbose ) puts( "Avoid START" );
+ avoidcount = random() % avoidduration + avoidduration;
+
+ if( minleft < minright )
+ {
+ pos->SetTurnSpeed( -avoidturn );
+ if( verbose ) printf( "turning right %.2f\n", -avoidturn );
+ }
+ else
+ {
+ pos->SetTurnSpeed( +avoidturn );
+ if( verbose ) printf( "turning left %2f\n", +avoidturn );
+ }
+ }
+
+ avoidcount--;
+
+ return true; // busy avoding obstacles
+ }
+
+ return false; // didn't have to avoid anything
+ }
+
+
+ void Work()
+ {
+ if( ! ObstacleAvoid() )
+ {
+ if( verbose ) puts( "Cruise" );
+
+ //avoidcount = 0;
+ pos->SetXSpeed( cruisespeed );
+
+ Pose pose = pos->GetPose();
+
+ int x = (pose.x + 8) / 4;
+ int y = (pose.y + 8) / 4;
+
+// // oh what an awful bug - 5 hours to track this down. When using
+// // this controller in a world larger than 8*8 meters, a_goal can
+// // sometimes be NAN. Causing trouble WAY upstream.
+// if( x > 3 ) x = 3;
+// if( y > 3 ) y = 3;
+// if( x < 0 ) x = 0;
+// if( y < 0 ) y = 0;
+
+
+ Model* goal = pos->GetFlagCount() ? sink : source;
+ Pose gp = goal->GetPose();
+
+ //printf( "seeking %s\n", goal->Token() );
+
+ double a_goal =
+ // dtor( ( pos->GetFlagCount() ) ? have[y][x] : need[y][x] );
+ atan2( gp.y - pose.y, gp.x - pose.x );
+
+ // if we are low on juice - find the direction to the recharger instead
+ if( Hungry() )
+ {
+ //puts( "hungry - using refuel map" );
+
+ // use the refuel map
+ a_goal = dtor( refuel[y][x] );
+
+ if( charger_ahoy ) // I see a charger while hungry!
+ mode = MODE_DOCK;
+ }
+
+ double a_error = normalize( a_goal - pose.a );
+ pos->SetTurnSpeed( a_error );
+ }
+ }
+
+
+ // inspect the laser data and decide what to do
+ static int LaserUpdate( ModelLaser* laser, Robot* robot )
+ {
+ // if( laser->power_pack && laser->power_pack->charging )
+ // printf( "model %s power pack @%p is charging\n",
+ // laser->Token(), laser->power_pack );
+
+ if( laser->GetSamples(NULL) == NULL )
+ return 0;
+
+ switch( robot->mode )
+ {
+ case MODE_DOCK:
+ //puts( "DOCK" );
+ robot->Dock();
+ break;
+
+ case MODE_WORK:
+ //puts( "WORK" );
+ robot->Work();
+ break;
+
+ case MODE_UNDOCK:
+ //puts( "UNDOCK" );
+ robot->UnDock();
+ break;
+
+ default:
+ printf( "unrecognized mode %u\n", robot->mode );
+ }
+
+ return 0;
+ }
+
+ bool Hungry()
+ {
+ // XX
+ return false;
+
+ // return( pos->FindPowerPack()->ProportionRemaining() < 0.25 );
+ }
+
+ bool Full()
+ {
+ return( pos->FindPowerPack()->ProportionRemaining() > 0.95 );
+ }
+
+ static int PositionUpdate( ModelPosition* pos, Robot* robot )
+ {
+ Pose pose = pos->GetPose();
+
+#if 0
+ // when countdown reaches zero
+ if( --robot->node_interval_countdown == 0 )
+ {
+ // reset countdown
+ robot->node_interval_countdown = robot->node_interval;
+
+ Node* node = new Node( pose );
+ robot->graph.AddNode( node );
+
+ if( robot->last_node )
+ robot->last_node->AddEdge( new Edge( node ) );
+
+ robot->last_node = node;
+
+ // limit the number of nodes
+ while( robot->graph.nodes.size() > TRAIL_LENGTH_MAX )
+ robot->graph.PopFront();
+ }
+#endif
+
+ //printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
+ // pose.x, pose.y, pose.z, pose.a );
+
+ //pose.z += 0.0001;
+ //robot->pos->SetPose( pose );
+
+ // if we're close to the source we get a flag
+ Pose sourcepose = robot->source->GetPose();
+ Geom sourcegeom = robot->source->GetGeom();
+
+ if( hypot( sourcepose.x-pose.x, sourcepose.y-pose.y ) < sourcegeom.size.x/2.0 &&
+ pos->GetFlagCount() < PAYLOAD )
+ {
+ if( ++robot->work_get > workduration )
+ {
+ // transfer a chunk from source to robot
+ pos->PushFlag( robot->source->PopFlag() );
+ robot->work_get = 0;
+ }
+ }
+
+ robot->at_dest = false;
+
+ // if we're close to the sink we lose a flag
+ Pose sinkpose = robot->sink->GetPose();
+
+ if( hypot( sinkpose.x-pose.x, sinkpose.y-pose.y ) < sourcegeom.size.x/2.0 &&
+ pos->GetFlagCount() )
+ {
+ robot->at_dest = true;
+
+ if( ++robot->work_put > workduration )
+ {
+ //puts( "dropping" );
+ // transfer a chunk between robot and goal
+ robot->sink->PushFlag( pos->PopFlag() );
+ robot->work_put = 0;
+ }
+ }
+
+
+ //printf( "diss: %.2f\n", pos->FindPowerPack()->GetDissipated() );
+
+ return 0; // run again
+ }
+
+
+
+ static int FiducialUpdate( ModelFiducial* mod, Robot* robot )
+ {
+ robot->charger_ahoy = false;
+
+ std::vector<ModelFiducial::Fiducial>& fids = mod->GetFiducials();
+
+ for( unsigned int i = 0; i < fids.size(); i++ )
+ {
+ //printf( "fiducial %d is %d at %.2f m %.2f radians\n",
+ // i, f->id, f->range, f->bearing );
+
+ if( fids[i].id == 2 ) // I see a charging station
+ {
+ // record that I've seen it and where it is
+ robot->charger_ahoy = true;
+ robot->charger_bearing = fids[i].bearing;
+ robot->charger_range = fids[i].range;
+ robot->charger_heading = fids[i].geom.a;
+
+ //printf( "charger at %.2f radians\n", robot->charger_bearing );
+ break;
+ }
+ }
+
+ return 0; // run again
+ }
+
+
+ static int FlagIncr( Model* mod, Robot* robot )
+ {
+ printf( "model %s collected flag\n", mod->Token() );
+ return 0;
+ }
+
+ static int FlagDecr( Model* mod, Robot* robot )
+ {
+ printf( "model %s dropped flag\n", mod->Token() );
+ return 0;
+ }
+};
+
+
+unsigned int Robot::map_width( 100 );
+unsigned int Robot::map_height( 100 );
+uint8_t* Robot::map_data( NULL );
+
+void split( const std::string& text, const std::string& separators, std::vector<std::string>& words)
+{
+ int n = text.length();
+ int start, stop;
+ start = text.find_first_not_of(separators);
+ while ((start >= 0) && (start < n))
+ {
+ stop = text.find_first_of(separators, start);
+ if ((stop < 0) || (stop > n)) stop = n;
+ words.push_back(text.substr(start, stop - start));
+ start = text.find_first_not_of(separators, stop+1);
+ }
+}
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod, CtrlArgs* args )
+{
+ //printf( "%s args: %s\n", mod->Token(), args->worldfile.c_str() );
+
+ // tokenize the argument string into words
+ std::vector<std::string> words;
+ split( args->worldfile, std::string(" \t"), words );
+
+ //printf( "words size %u\n", words.size() );
+ //FOR_EACH( it, words )
+ //printf( "word: %s\n", it->c_str() );
+ //puts( "" );
+
+ // expecting a task color name as the 1th argument
+ assert( words.size() == 2 );
+ assert( words[1].size() > 0 );
+
+ new Robot( (ModelPosition*)mod,
+ mod->GetWorld()->GetModel( words[1] + "_source" ),
+ mod->GetWorld()->GetModel( words[1] + "_sink" ));
+
+ return 0; //ok
+}
+
+
+
View
4 examples/ctrl/source.cc
@@ -3,6 +3,7 @@ using namespace Stg;
const int INTERVAL = 100;
const double FLAGSZ = 0.4;
+const unsigned int CAPACITY = 1;
int Update( Model* mod, void* dummy );
@@ -18,7 +19,8 @@ extern "C" int Init( Model* mod )
// inspect the laser data and decide what to do
int Update( Model* mod, void* dummy )
{
- if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
+ if(( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
+ && ( mod->GetFlagCount() < CAPACITY) )
mod->PushFlag( new Model::Flag( Color( 1,1,0 ), FLAGSZ ) );
return 0; // run again
View
8 libstage/stage.hh
@@ -317,6 +317,12 @@ namespace Stg
z + p.z,
normalize(a + p.a) );
}
+
+ // a < b iff a is closer to the origin than b
+ bool operator<( const Pose& other ) const
+ {
+ return( hypot( y, x ) < hypot( other.y, other.x ));
+ }
};
@@ -504,7 +510,7 @@ namespace Stg
void RegisterModels();
- /** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */
+ /** Abstract class for adding visualizations to models. Visualize must be overloaded, and is then called in the models local coord system */
class Visualizer {
private:
const std::string menu_name;
View
253 worlds/fasr2.world
@@ -0,0 +1,253 @@
+
+# FASR demo world
+# Authors: Richard Vaughan
+# $Id: fasr.world,v 1.4 2008-04-01 23:57:41 rtv Exp $
+
+include "pioneer.inc"
+include "map.inc"
+include "sick.inc"
+
+speedup 10.000
+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
+
+resolution 0.02
+
+threads 0
+
+# configure the GUI window
+window
+(
+ size [ 980.000 1016.000 ]
+
+ center [ -15.341 4.193 ]
+ rotate [ 0 0 ]
+ scale 13.528
+
+ show_data 1
+ show_flags 1
+
+ # interval 50
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "cave"
+ pose [0 0 0 0]
+ size [100.000 100.000 0.600]
+ bitmap "bitmaps/cave.png"
+)
+
+zone
+(
+ color "green"
+ pose [ 38.682 -25.820 0 0 ]
+ name "green_source"
+ ctrl "source"
+)
+
+zone( color "DarkGreen"
+ pose [ -10.930 44.453 0 0 ]
+ name "green_sink"
+ ctrl "sink"
+)
+
+zone( color "blue"
+ pose [ -40.264 -18.084 0 0 ]
+ name "blue_source"
+ ctrl "source"
+)
+
+zone( color "DarkBlue"
+ pose [ 39.986 37.517 0 0 ]
+ name "blue_sink"
+ ctrl "sink"
+)
+
+zone( color "red"
+ pose [ -41.325 32.878 0 0 ]
+ name "red_source"
+ ctrl "source"
+)
+
+zone( color "DarkRed"
+ pose [ 40.730 22.450 0 0 ]
+ name "red_sink"
+ ctrl "sink"
+)
+
+zone( color "yellow"
+ pose [ -37.146 -39.531 0 0 ]
+ name "yellow_source"
+ ctrl "source"
+)
+
+zone( color_rgba [ 0.8 0.8 0 1 ]
+ pose [ -38.243 9.591 0 0 ]
+ name "yellow_sink"
+ ctrl "sink"
+)
+
+zone( color "cyan"
+ pose [ 26.255 41.679 0 0 ]
+ name "cyan_source"
+ ctrl "source"
+)
+
+zone( color "DarkCyan"
+ pose [ 9.412 -40.363 0 0 ]
+ name "cyan_sink"
+ ctrl "sink"
+)
+
+zone( color "magenta"
+ pose [ -14.246 -42.150 0 0 ]
+ name "magenta_source"
+ ctrl "source"
+)
+
+zone( color "DarkMagenta"
+ pose [ 38.511 -14.808 0 0 ]
+ name "magenta_sink"
+ ctrl "sink"
+)
+
+
+define charging_bump model
+(
+ pose [0.240 0 -0.100 0 ]
+ size [0.120 0.050 0.100]
+ take_watts 2000.0
+ color "orange"
+ obstacle_return 0
+)
+
+define charge_station model
+(
+ size [ 0.100 0.300 0.100 ]
+ color "purple"
+
+ # side blocks to restrict view angle
+ model( color "purple" size [0.100 0.050 0.250] pose [ 0 0.100 0 0 ] )
+ model( color "purple" size [0.100 0.050 0.250] pose [ 0 -0.100 0 0 ] )
+
+ # the charging block
+ model(
+ pose [ 0.010 0 0 0 ]
+ color "yellow"
+ size [ 0.050 0.200 0.100 ]
+ joules -1 # provides infinite energy
+ give_watts 2000
+ fiducial_return 2 # look for this in the fiducial sensor
+
+ alwayson 1 # so we give charge without any explicit subscriber
+ )
+
+)
+
+#charge_station( pose [ 7.908 -2.510 0 0 ] )
+#charge_station( pose [ 7.920 -3.847 0 0 ] )
+#charge_station( pose [ 7.449 -4.974 0 -40.107 ] )
+#charge_station( pose [ 7.698 -1.432 0 31.513 ] )
+#charge_station( pose [ 5.162 -0.814 0 88.809 ] )
+
+#charge_station( pose [ 3.927 -0.693 0 88.808 ] )
+#charge_station( pose [ 6.427 -0.964 0 74.484 ] )
+#charge_station( pose [ 4.964 -0.752 0 88.808 ] )
+#charge_station( pose [ 3.832 -0.704 0 91.673 ] )
+#charge_station( pose [ 7.949 -4.019 0 0 ] )
+
+define autorob pioneer2dx
+(
+ sicklaser( samples 32 range_max 5 laser_return 2 watts 30 )
+# ctrl "fasr2"
+ joules 100000
+ joules_capacity 400000
+ fiducial_return 0
+ charging_bump( fiducial( range_max 5 pose [ 0 0 -0.100 0 ] ) )
+
+ # small speed optimization
+ # trail_length 0
+)
+
+define redbot autorob( color "red" ctrl "fasr2 red" )
+define greenbot autorob( color "green" ctrl "fasr2 green" )
+define bluebot autorob( color "blue" ctrl "fasr2 blue" )
+define cyanbot autorob( color "cyan" ctrl "fasr2 cyan" )
+define yellowbot autorob( color "yellow" ctrl "fasr2 yellow" )
+define magentabot autorob( color "magenta" ctrl "fasr2 magenta" )
+
+#define bot autorob( ctrl "fasr2 " )"
+#define bot autorob( ctrl "fasr2 " )"
+#define bot autorob( ctrl "fasr2 " )"
+
+redbot( pose [-10.909 12.012 0 125.831] joules 100000 )
+redbot( pose [-11.699 11.274 0 125.831] joules 100000 )
+redbot( pose [-11.356 10.669 0 125.831] joules 100000 )
+redbot( pose [-10.066 11.432 0 125.831] joules 100000 )
+redbot( pose [-10.698 10.458 0 125.831] joules 100000 )
+redbot( pose [-9.987 10.669 0 125.831] joules 100000 )
+redbot( pose [-11.593 12.222 0 125.831] joules 100000 )
+redbot( pose [-10.303 12.222 0 125.831] joules 100000 )
+redbot( pose [-12.120 12.012 0 125.831] joules 100000 )
+redbot( pose [-10.777 11.353 0 125.831] joules 100000 )
+
+bluebot( pose [-11.191 6.965 0 174.453] joules 200000 )
+bluebot( pose [-11.270 8.097 0 174.453] joules 200000 )
+bluebot( pose [-10.295 6.886 0 174.453] joules 200000 )
+bluebot( pose [-8.900 8.361 0 174.453] joules 200000 )
+bluebot( pose [-10.374 8.940 0 174.453] joules 200000 )
+bluebot( pose [-9.321 7.650 0 174.453] joules 200000 )
+bluebot( pose [-9.453 9.072 0 174.453] joules 200000 )
+bluebot( pose [-11.401 8.966 0 174.453] joules 200000 )
+bluebot( pose [-9.558 6.570 0 174.453] joules 200000 )
+bluebot( pose [-10.348 7.992 0 174.453] joules 200000 )
+
+#greenbot( pose [-4.059 16.220 0 -117.456] joules 400000 )
+#greenbot( pose [-2.610 16.957 0 -117.456] joules 400000 )
+#greenbot( pose [-1.794 15.087 0 -117.456] joules 400000 )
+#greenbot( pose [-4.164 15.272 0 -117.456] joules 400000 )
+#greenbot( pose [-3.348 14.771 0 -117.456] joules 400000 )
+#greenbot( pose [-2.242 15.930 0 -117.456] joules 400000 )
+#greenbot( pose [-3.348 17.036 0 -117.456] joules 400000 )
+#greenbot( pose [-4.322 17.510 0 -117.456] joules 400000 )
+#greenbot( pose [-4.822 16.536 0 -117.456] joules 400000 )
+#greenbot( pose [-4.980 15.693 0 -117.456] joules 400000 )
+
+#cyanbot( pose [-7.576 4.137 0 44.991] joules 100000 )
+#cyanbot( pose [-8.459 4.269 0 44.991] joules 100000 )
+#cyanbot( pose [-9.907 3.766 0 44.991] joules 100000 )
+#cyanbot( pose [-10.640 2.698 0 44.991] joules 100000 )
+#cyanbot( pose [-8.557 1.729 0 44.991] joules 100000 )
+#cyanbot( pose [-9.821 1.505 0 44.991] joules 100000 )
+#cyanbot( pose [-7.926 3.090 0 44.991] joules 100000 )
+#cyanbot( pose [-9.038 2.865 0 44.991] joules 100000 )
+#cyanbot( pose [-11.097 0.973 0 44.991] joules 100000 )
+#cyanbot( pose [-11.673 2.537 0 44.991] joules 100000 )
+
+#yellowbot( pose [-15.452 7.366 0 35.145] joules 200000 )
+#yellowbot( pose [-13.978 6.340 0 35.145] joules 200000 )
+#yellowbot( pose [-16.163 6.524 0 35.145] joules 200000 )
+#yellowbot( pose [-15.979 5.550 0 35.145] joules 200000 )
+#yellowbot( pose [-14.873 5.208 0 35.145] joules 200000 )
+#yellowbot( pose [-14.504 7.419 0 35.145] joules 200000 )
+#yellowbot( pose [-16.374 8.025 0 35.145] joules 200000 )
+#yellowbot( pose [-15.189 8.420 0 35.145] joules 200000 )
+#yellowbot( pose [-13.793 5.576 0 35.145] joules 200000 )
+#yellowbot( pose [-15.004 6.340 0 35.145] joules 200000 )
+
+#magentabot( pose [-7.850 14.772 0 -143.181] joules 300000 )
+#magentabot( pose [-8.323 14.087 0 -143.181] joules 300000 )
+#magentabot( pose [-7.850 13.613 0 -143.181] joules 300000 )
+#magentabot( pose [-7.139 14.772 0 -143.181] joules 300000 )
+#magentabot( pose [-6.401 14.061 0 -143.181] joules 300000 )
+#magentabot( pose [-7.165 13.429 0 -143.181] joules 300000 )
+#magentabot( pose [-6.428 13.323 0 -143.181] joules 300000 )
+#magentabot( pose [-7.455 15.430 0 -143.181] joules 300000 )
+#magentabot( pose [-6.507 14.851 0 -143.181] joules 300000 )
+#magentabot( pose [-8.718 14.877 0 -143.181] joules 300000 )
+
View
2 worlds/map.inc
@@ -22,7 +22,7 @@ define floorplan model
define zone model
(
color "orange"
- size [ 2 2 0.02 ]
+ size [ 4 4 0.02 ]
gui_nose 0
gui_grid 0

0 comments on commit 7e37217

Please sign in to comment.