#include <sys/time.h>#include <time.h>#include "fsmmove.h"#include "trgen.h"#include "balet.h"#include "robot.h"#include <pthread.h>#include <path_planner.h>#include <signal.h>#include "movehelper.h"#include <sharp.h>#include <unistd.h>#include <map.h>#include "robot_config.h"
Go to the source code of this file.
Defines | |
| #define | FSM_MOTION |
| #define | OBS_PERIOD_WATCH 100000 |
| Time period for watching the sensors in us. | |
| #define | OBS_PERIOD_BETWEEN_DET 1000000 |
| Minimal time period between two detections in us. | |
| #define | OBS_CSPACE (200/MAP_CELL_SIZE) |
| Configuration Space of the obstacle in cell units. | |
| #define | OBS_FORGET_PERIOD 100000 |
| The period of thread_update_map(). | |
| #define | OBS_UPDATE_MAP_SPEED 15 |
| Speed to forget the obstacles. | |
| #define | MAX_POS_ERROR_M 0.5 |
| #define | CLOSE_TO_TARGET_M 0.1 |
| #define | MAX_WAIT_FOR_CLOSE_MS 2000 |
| #define | MOTION_PERIOD_NS (50/*ms*/*1000*1000) |
| #define | SIG_TIMER (SIGRTMIN+0) |
| #define | SIG_NEWDATA (SIGRTMIN+1) |
Functions | |
| static fsm_event | new_goal (void) |
| static void | stop () |
| void | delete_actual_trajectory () |
| static void | notify_fsm (bool done, double error) |
| Sends events from follower thread to FSM. | |
| static void * | trajectory_follower (void *arg) |
| A thread running the controller. | |
| static void * | trajctory_recalc (void *arg) |
| A thread running the trajectory recalc. | |
| static void * | thread_update_map (void *arg) |
| A thread updating the map. | |
| static void | go (Trajectory *t) |
Tells trajctory_follower to start moving along trajectory t. | |
| static int | new_trajectory (void) |
| Starts moving on trajectory prepared by functions in movehelper.cc. | |
| FSM_STATE_DECL (movement) | |
| FSM_STATE_DECL (close_to_target) | |
| FSM_STATE_DECL (wait_for_command) | |
| FSM_STATE (move_init) | |
| FSM_STATE (wait_for_command) | |
| FSM_STATE (movement) | |
| FSM_STATE (close_to_target) | |
Variables | |
| const double | k [3] |
| static pthread_t | thr_trajctory_follower |
| static pthread_t | thr_trajctory_recalc |
| static pthread_t | thr_update_map |
| static struct timeval | tv_start |
| Absolute time, when trajectory started. | |
| static Trajectory * | actual_trajectory = NULL |
| Stores the actually followed trajectory object. | |
| bool | actual_trajectory_reverse |
| pthread_mutex_t | actual_trajectory_lock = PTHREAD_MUTEX_INITIALIZER |
| pthread_mutex_t | switch_to_trajectory_lock = PTHREAD_MUTEX_INITIALIZER |
Definition in file fsmmove.cc.
| #define OBS_CSPACE (200/MAP_CELL_SIZE) |
Configuration Space of the obstacle in cell units.
In this case 200mm = 2 cells.
Definition at line 30 of file fsmmove.cc.
Referenced by trajctory_recalc().
| #define OBS_PERIOD_BETWEEN_DET 1000000 |
Minimal time period between two detections in us.
Definition at line 29 of file fsmmove.cc.
Referenced by trajctory_recalc().
| #define OBS_UPDATE_MAP_SPEED 15 |
Speed to forget the obstacles.
Definition at line 33 of file fsmmove.cc.
Referenced by thread_update_map().
| static void go | ( | Trajectory * | t | ) | [static] |
Tells trajctory_follower to start moving along trajectory t.
| t | Trajectory to follow. |
Definition at line 323 of file fsmmove.cc.
References actual_trajectory_lock, and tv_start.
Referenced by new_goal(), and new_trajectory().
| static int new_goal | ( | void | ) | [static] |
Calculates and simplify a path to goal position avoiding obstacles. In case of error it sends the proper event to MAIN FSM.
Get actual position.
Get goal position.
Calculates the path between actual position to goal position. If the goal is not reachable returns EV_GOAL_NOT_REACHABLE.
Definition at line 347 of file fsmmove.cc.
References Trajectory::addPoint(), DBG, Trajectory::finalHeading, freePathMemory(), FSM_SIGNAL, go(), _PathPoint::next, path_planner(), Pos::phi, PP_ERROR_GOAL_IS_OBSTACLE, PP_ERROR_GOAL_NOT_REACHABLE, PP_ERROR_MAP_NOT_INIT, Trajectory::prepare(), ROBOT_LOCK, ROBOT_UNLOCK, trajectoryConstraintsDefault, _PathPoint::x, Pos::x, _PathPoint::y, and Pos::y.
Referenced by FSM_STATE(), and trajctory_recalc().
| static int new_trajectory | ( | void | ) | [static] |
Starts moving on trajectory prepared by functions in movehelper.cc.
In case of error it sends the proper event to MAIN FSM.
Definition at line 429 of file fsmmove.cc.
References DBG, FSM_SIGNAL, go(), Pos::phi, Trajectory::prepare(), ROBOT_LOCK, ROBOT_UNLOCK, Pos::x, and Pos::y.
Referenced by FSM_STATE(), robot_trajectory_add_final_point(), robot_trajectory_add_point(), and robot_trajectory_new_ex().
| static void notify_fsm | ( | bool | done, | |
| double | error | |||
| ) | [static] |
Sends events from follower thread to FSM.
Definition at line 99 of file fsmmove.cc.
References CLOSE_TO_TARGET_M, FSM_SIGNAL, and MAX_POS_ERROR_M.
Referenced by trajectory_follower().
| static void* thread_update_map | ( | void * | arg | ) | [static] |
A thread updating the map.
Updates Map.
Definition at line 310 of file fsmmove.cc.
References OBS_FORGET_PERIOD, OBS_UPDATE_MAP_SPEED, and ShmapUpdateTmpObstacles().
Referenced by FSM_STATE().
| static void* trajctory_recalc | ( | void * | arg | ) | [static] |
A thread running the trajectory recalc.
This (low-medium priority) thread updates the map with sensors information. If it is necesary, it recalculate the path.
| arg |
Updates Map.
Get actual position.
The obstacle cell its calulated as (val*cos(p.phi)) + p.x) and ((val*sin(p.phi)) + p.y) where p is actual position and val is the value of the sensors in milimiters. Then ShmapPoint2Cell_X() and ShmapPoint2Cell_Y() funcions are used to get cell coordonate.
Then all the cells arround obstacle cell are set as MAP_NEW_OBSTACLE. If there are a path cell between them, the path will be recalculated.
Definition at line 229 of file fsmmove.cc.
References DBG, FSM_SIGNAL, MAP_FREE, MAP_GOAL, MAP_NEW_OBSTACLE, MAP_PATH, MAP_WALL, new_goal(), OBS_CSPACE, OBS_PERIOD_BETWEEN_DET, OBS_PERIOD_WATCH, Pos::phi, ROBOT_LOCK, ROBOT_UNLOCK, ShmapGetCellValue(), ShmapIsFreePoint(), ShmapPoint2Cell_X(), ShmapPoint2Cell_Y(), ShmapSetCellValue(), Pos::x, and Pos::y.
Referenced by FSM_STATE().
| static void* trajectory_follower | ( | void * | arg | ) | [static] |
A thread running the controller.
This (high priority) thread executes the motion control algorithm. It calculates repference position based on actual trajectory and current time. Then it calls "balet" controller to close feedback.
| arg |
Definition at line 131 of file fsmmove.cc.
References actual_trajectory_lock, DBG, Trajectory::getRefPos(), k, MOTION_PERIOD_NS, notify_fsm(), position_type::phi, Pos::phi, ROBOT_LOCK, robot_send_speed(), ROBOT_UNLOCK, SIG_NEWDATA, SIG_TIMER, tv_start, Pos::v, position_type::x, Pos::x, position_type::y, and Pos::y.
Referenced by FSM_STATE().
| const double k[3] |
Initial value:
{
2,
2,
10
}
Definition at line 40 of file fsmmove.cc.
Referenced by trajectory_follower().
struct timeval tv_start [static] |
Absolute time, when trajectory started.
Definition at line 60 of file fsmmove.cc.
Referenced by go(), and trajectory_follower().
1.5.3