/**
* @file map.c
- * @brief Useful functions related map
+ * @brief Useful functions related map
* @author Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>
- *
+ *
* This file contains functions to manage map.
* The map should be accessed only by this library.
*
/** @addtogroup maplib */
/**
* @{
- *
+ *
*/
* @name Shared Memory Map related functions
* @{
*/
-/**
+/**
* @brief Init Shared Map memory
*
* @param init_flag 1 to init the map memory and set all cells with
const int shmap_size = sizeof(struct map);
if (map == NULL){
// Allocate memory to shared map
- shmap_id = shmget (SHM_MAP_KEY , shmap_size, IPC_CREAT | S_IRUSR | S_IWUSR); // Removed flag IPC_EXCL
-
+ shmap_id = shmget (SHM_MAP_KEY , shmap_size, IPC_CREAT | S_IRUSR | S_IWUSR); // Removed flag IPC_EXCL
+
if (shmap_id == -1) {
perror("shmget");
exit(1);
}
-
+
/* Attach the shared memory segment. */
map = (struct map *) shmat (shmap_id, 0, 0);
if ((int)map == -1) {
perror("shmat");
exit(1);
}
-
+
/* Initialize Map Memory */
if (init_flag) ShmapAllFreeSpace();
-
+
//printf("Map initialized\n");
}
return map;
}
-/**
+/**
* @brief Free Shared Map memory
*/
void ShmapFree(void){
shmctl (shmap_id, IPC_RMID, 0);
}
-/**
+/**
* @brief Deatach Shared Map memory
*/
void ShmapDt(void){
shmdt (map);
}
-/**
+/**
* @brief Check if Shared Map memory is init
* @return Pointer to the map or NULL if not initialized.
*/
* @brief Creates an obstacle in the map with a square shape
* @param xs Coordonate X (in m) of the central point
* @param ys Coordonate Y (in m) of the central point
- * @param r Radius (in m)
+ * @param r Radius (in m)
* @param cell Type of obstacle.
- *
+ *
*/
int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
{
// define a circumscribe square - just to shorten cyclic pass through the playground
//ShmapPoint2Cell(xs-r, ys-r, &i1, &j1, &valid);
//ShmapPoint2Cell(xs+r, ys+r, &i2, &j2, &valid);
-
+
i1 = 0;
i2 = MAP_WIDTH;
j1 = 0;
* @param x2 Coordonate X (in m) of the second point
* @param y2 Coordonate Y (in m) of the second point
* @param cell Type of obstacle.
- *
+ *
*/
int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
{
i2 = i1;
i1 = i;
}
-
+
if (j1 > j2) {
j = j2;
j2 = j1;
j1 = j;
}
-
+
//DBG("Drawing a rectangle between points (%d, %d) and (%d,%d)\n", init_i,init_j,limit_i,limit_j);
for(i=i1; i<=i2; i++) {
for(j=j1; j<=j2; j++) {
int xx, yy;
xx = (int)floor(x/MAP_CELL_SIZE_M);
yy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
-
+
if (valid != NULL) {
*valid = ( ( xx < MAP_WIDTH ) && (yy < MAP_HEIGHT) && ( xx >= 0 ) && ( yy >= 0) );
}
*/
int ShmapCell2Point(int ix, int iy, double *x, double *y)
{
- if ( (ix >= 0) && (ix <= MAP_WIDTH-1)) {
+ if ( (ix >= 0) && (ix <= MAP_WIDTH-1)) {
if (x) *x = (ix+0.5)*MAP_CELL_SIZE_M;
}
else return -1;
interprocess comunication methods.
* - Discrete Space (@ref discrete_map): the real space is divided
into cells of variable size. Each cell has its own properties.
- *
+ *
* @section shmap Shared Memory Map
* There are two important methods to access the shared map. Before
* @warning If the program exit without a ShmapFree(), the memory will
* stay in Linux SHM. You can verify it with command "ipcs -m". For
* further details, read shm documentation.
- *
+ *
* @section the_map The map
* @subsection real_map The real space
- *
+ *
* The map size is (#MAP_PLAYGROUND_WIDTH_MM
* x #MAP_PLAYGROUND_HEIGHT_MM). This space discretization is
* explained in section @ref discrete_map .
- *
+ *
* @code
* 0 MAP_PLAYGROUND_WIDTH_MM
* MAP_PLAYGROUND_HEIGHT_MM +---------------------------------------+
* There are only a coordonates difference between points and
* cells. Cell which cordonates [0,0] represents all the points
* contained in a square sited up left corner.
- *
+ *
* @code
* X 0 1 2 3 4 5 6 7 8 MAP_WIDTH-1
* Y +---+---+---+---+---+---+---+---+---+---+
* +---+---+---+---+---+---+---+---+---+---+
* MAP_HEIGHT-1
* @endcode
- *
+ *
* @note Matrix convention is usally (row, column). In this program
* the convention will be (column, row) in order to keep real
* coordonates relation. C language stockes matrix in rows, so a cell
* coordonate (x,y) will be in a C matrix [y][x].
- *
+ *
* @section map_content Map Content
* Each cell/point of the map (::MapCell) contains two types of information:
* - Value (::MapCellValue) : It determins the type of cell. It is the
information used by A* algorithm.
* - Flag (::MapCellFlag) : It is extra information.
- *
+ *
* @subsection cell_values Map Values
- *
+ *
* This information is of the type ::MapCellValue.The value that the
* map contains can be:
* - #MAP_WALL: Wall of the map. A fixed obstacle.
sensors found an obstacle. It can be view as a moving obstacle.
* - #MAP_NEW_OBSTACLE_CSPACE: Configuration Space of the obstacle .
* - #MAP_FREE: Free space.
- *
+ *
* One special type is #MAP_NOT_IN_MAP. It is a error code return when
* we try to acces to a space wich is not in the map. (i.e. the
* request cell/point exceeds map dimensions.)
* @note The configuration space is a special obstacle type that
* depends on the geometry of the robot. For more information, read
* a book about robotics.
- *
+ *
* @subsection cell_flags Cell Flags
- * The possible cell flags are:
- * - #MAP_FLAG_NO_FLAG
- * - #MAP_FLAG_WALL
- * - #MAP_FLAG_PATH
- * - #MAP_FLAG_START
- * - #MAP_FLAG_GOAL
+ * The possible cell flags are:
+ * - #MAP_FLAG_NO_FLAG
+ * - #MAP_FLAG_WALL
+ * - #MAP_FLAG_PATH
+ * - #MAP_FLAG_START
+ * - #MAP_FLAG_GOAL
* One special type is #MAP_FLAG_ERROR. It is a error code return when
* we try to acces to a space wich is not in the map. (i.e. the
* request cell/point exceeds map dimensions.)
* @subsection map_func Accessing to map information
- *
+ *
* FIXME: Obsolete: ShmapGetPointValue(), ShmapSetPointValue(),
* ShmapSetPointValue(), ShmapSetCellValue() do not exist
*
* To read and write cell values, use functions ShmapGetCellValue()
* and ShmapSetCellValue(). If you want to acces directly to points,
* you can also use ShmapGetPointValue() and ShmapSetPointValue().
- *
+ *
* There are similar functions to get and set flags:
* ShmapGetCellFlag() and ShmapSetCellFlag(), ShmapGetPointValue() and
* ShmapSetPointFlag().
- *
+ *
* It can be possible also to edit more than a point at the same time
* with function ShmapSetRectangleType().
- *
+ *
* If you want to know if a cell is free see ShmapIsFreeCell() and
* ShmapIsFreePoint().
- *
+ *
* ShmapUpdateTmpObstacles() is a specific function created for
* Eurobot Project. Please, see an example in testmap.c file
- *
+ *
* @example testmap.c
* Example how to use Shmap library.
*/
* @name Cell Flags
* @{
*/
-#define MAP_FLAG_WALL 1 /**< Known wall */
-#define MAP_FLAG_PATH 2
-#define MAP_FLAG_START 4
-#define MAP_FLAG_GOAL 8
-#define MAP_FLAG_DET_OBST 16 /**< Set when an obstacle is detected, cleard on every "map forget cycle" */
-#define MAP_FLAG_SIMULATED_WALL 32 /**< Used by robomon to simulate obstacles */
-#define MAP_FLAG_IGNORE_OBST 64 /**< If obstacle is detected here, ignore it */
-
-/** "Safety margin" around obstacle - used only during A* planning and
- * not during runtime obstacle avoidance. */
-#define MAP_FLAG_PLAN_MARGIN 128
+#define MAP_FLAG_WALL 1 /**< Known wall */
+#define MAP_FLAG_PATH 2
+#define MAP_FLAG_START 4
+#define MAP_FLAG_GOAL 8
+#define MAP_FLAG_DET_OBST 16 /**< Set when an obstacle is detected, cleard on every "map forget cycle" */
+#define MAP_FLAG_SIMULATED_WALL 32 /**< Used by robomon to simulate obstacles */
+#define MAP_FLAG_IGNORE_OBST 64 /**< If obstacle is detected here, ignore it */
+#define MAP_FLAG_PLAN_MARGIN 128 /**< Safety margin around obstacle,
+ * do not plan path here,
+ * motion controler is not allowed to enter this area */
/** @}*/
/** @name Shared Memory macros */
/**@{*/
-#define SHM_MAP_KEY 555 /**< Key use to share the memory SHM*/
+#define SHM_MAP_KEY 555 /**< Key use to share the memory SHM*/
/**@}*/
/**@}*/
#ifdef __cplusplus
extern "C" {
-#endif
+#endif
typedef unsigned int map_cell_detobst_t;
typedef unsigned int map_cell_flag_t;
map_cell_detobst_t detected_obstacle; /**< 0 = no obstacle, 255 = new obstacle */
} MapCell;
-#define MAP_NEW_OBSTACLE 255
-#define MAP_NO_OBSTACLE 0
+#define MAP_NEW_OBSTACLE 255
+#define MAP_NO_OBSTACLE 0
struct map {
MapCell cells[MAP_HEIGHT][MAP_WIDTH];
-
+
}; /**< The main structure.*/
/** See ShmapCellAtPoint(). */
extern struct map_cell ShmapNoCell;
-
+
void ShmapAllFreeSpace(void);
struct map *ShmapInit(int init_flag);
void ShmapFree(void);
/**
* Gives information about a cell
- *
+ *
* @param x Coordinate of a cell
* @param y Coordinate of a cell
* @return 1 if cell is in map, 0 otherwise
#ifdef __cplusplus
}
-#endif
+#endif
#endif //__MAP_H_
struct map *map = ShmapIsMapInit();
const struct { int x; int y; } neigh[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
{-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
-
+
for (yy=0;yy<MAP_HEIGHT;yy++) {
for (xx=0;xx<MAP_WIDTH;xx++) {
// check obstacles around this cell to determine safety zone flag
map->cells[yy][xx].flags &= ~MAP_FLAG_PLAN_MARGIN; // delete margin status
for (ii=0; ii< sizeof(neigh)/sizeof(neigh[0]); ii++)
{
- if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y)
- && (((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST)
+ if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y)
+ && (((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST)
|| (map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).detected_obstacle > 0 ))
{
// creates safety status
}
-/**
+/**
* Returns the number of points of the path on success or an error code.
* @param xstart_real X coordinate in meters
* @param ystart_real Y coordinate in meters
* It takes parameters of start and goal points. It returns the number
* of points of the trajectory and the caller must specify pointers to
* path and angle.
- *
+ *
* The result of the algorithm is stored in a list of ::PathPoint,
* terminated by NULL, containing the points of the path. This memory
* is allocated dynamicaly and its address will be returned as
* argument. After use the path, memory should be free with
* freePathMemory(). A pointer of the final angle must be also passed
* as argument.
- *
+ *
* The map memory must be init with ShmapInit() before calling
* path_planner(). Do not forget to free this map memory with
* ShmapFree().
PathPoint *original_path;
PathPoint * tmp;
static pthread_mutex_t plan_mutex = PTHREAD_MUTEX_INITIALIZER;
-
+
// Init the map
if (! ShmapIsMapInit()) {
DBG("ERROR: shared map memory is not init.\n");
}
pthread_mutex_lock(&plan_mutex);
-
+
// Call to A* algorithm
nbpoints = aAlgorithm(xstart_real,ystart_real, xgoal_real, ygoal_real, &astar_path);
if (nbpoints < 0) {
tmp--;
tmp->x = xgoal_real;
tmp->y = ygoal_real;
-
-
+
+
// DBG("Simplifing the path\n");
- // Calc a simplest path
+ // Calc a simplest path
(*simple_path) = (PathPoint *) malloc(sizeof(PathPoint));
count = path_simplifier(original_path, nbpoints, *simple_path, angle);
// Do not send the first point
(*simple_path)= (*simple_path)->next;
free(tmp);
free(original_path);
-
+
// All is OK
ret = 1;
out:
*/
static enum target_status new_goal(struct move_target *move_target, double start_in_future)
{
- enum target_status ret;
+ enum target_status ret;
double angle;
PathPoint * path = NULL;
PathPoint * tmp_point = NULL;
// Where to start trajectory planning?
get_future_pos(start_in_future, future_traj_point, time_ts);
Point start(future_traj_point.x, future_traj_point.y);
-
+
Trajectory *t = new Trajectory(move_target->tc, backward);
if (move_target->heading.operation == TOP_ARRIVE_FROM) {
ret = TARGET_ERROR; break;
case PP_ERROR_GOAL_IS_OBSTACLE:
case PP_ERROR_GOAL_NOT_REACHABLE:
- ret = TARGET_INACC; break;
+ ret = TARGET_INACC; break;
default:
ret = TARGET_OK; break;
}
delete(t);
ret = TARGET_ERROR;
}
-
+
return ret;
}
* movehelper.cc. In case of error it sends the proper event to MAIN
* FSM.
*
- * @return TARGET_OK on succes, TARGET_ERROR on error.
+ * @return TARGET_OK on succes, TARGET_ERROR on error.
*/
static enum target_status new_trajectory(Trajectory *t)
{
}
}
-/**
+/**
* Handles new target signal from main FSM
- *
- * @param target
- *
- * @return
+ *
+ * @param target
+ *
+ * @return
*/
static enum target_status new_target(struct move_target *target)
{
return ret;
}
-/**
+/**
* Recalculates trajectory to previous move target specified by
* new_target()
*
- * @return
+ * @return
*/
enum target_status
recalculate_trajectory(void)
/* TODO: Different start for planning than esitmated position */
enum target_status ret;
current_target.use_planning = true;
- ret = new_goal(¤t_target, TRGEN_DELAY);
- switch (ret) {
+ ret = new_goal(¤t_target, TRGEN_DELAY);
+ switch (ret) {
case TARGET_OK:
- break;
- case TARGET_ERROR:
- ul_logerr("Target error on recalculation_in_progress\n");
- break;
- case TARGET_INACC:
+ break;
+ case TARGET_ERROR:
+ ul_logerr("Target error on recalculation_in_progress\n");
+ break;
+ case TARGET_INACC:
break;
}
// We can go to the target:
case TARGET_OK: break;
// Target inaccessible because of an obstacle
- case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
+ case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
// Fatal error during planning
case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
ul_logerr("Target error\n");
case EV_EXIT:
stop();
break;
- case EV_OBSTACLE:
+ case EV_OBSTACLE:
case EV_RETURN:
case EV_TRAJECTORY_DONE:
case EV_NEW_TARGET:
* @file motion-control.cc
* @author Michal Sojka <sojkam1@fel.cvut.cz>, Petr Beneš
* @date Fri Mar 20 10:36:59 2009
- *
- * @brief
- *
- *
+ *
+ * @brief
+ *
+ *
*/
//#define MOTION_DEBUG
static Trajectory *actual_trajectory;
static pthread_mutex_t actual_trajectory_lock;
-// Trajectory recalculation
+// Trajectory recalculation
sem_t recalculation_not_running;
sem_t measurement_received;
perror("Warning: Cannot set RT priority for thread_prio_traj_follower()");
clock_gettime(CLOCK_REALTIME, &next);
-
+
while (1) {
ret = sem_timedwait(&measurement_received, &next);
-
+
if (ret == -1 && errno == ETIMEDOUT) {
next_period(&next, MOTION_PERIOD_NS);
if (measurement_ok) {
fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!");
}
measurement_ok--;
- }
+ }
} else {
next_period(&next, MEASURE_TIMEOUT_NS);
if (measurement_ok < 2) {
old = actual_trajectory;
gettimeofday(&tv_start, NULL);
actual_trajectory = t;
-#ifdef MOTION_LOG
+#ifdef MOTION_LOG
t->logTraj(tv_start.tv_sec + 1e-6*tv_start.tv_usec);
#endif
if (old)
{
pthread_mutex_lock(&switch_to_trajectory_lock);
switch_to_trajectory = t;
- switch_time = time;
+ switch_time = time;
pthread_mutex_unlock(&switch_to_trajectory_lock);
struct timeval tv;
sem_post(&measurement_received);
}
-/**
+/**
* Initializes motion controller.
- *
- *
+ *
+ *
* @return Zero on success, non-zero otherwise.
*/
int motion_control_init()
robot.orte.motion_speed.right = 0;
robot.orte.motion_speed.left = 0;
- ORTEPublicationSend(robot.orte.publication_motion_speed);
+ ORTEPublicationSend(robot.orte.publication_motion_speed);
}
projects / eurobot / public.git / commitdiff