src/services/motion/MotionService.js
'use strict';
/** @namespace services.motion */
const TaskError = Mep.require('strategy/TaskError');
const EventEmitter = require('events').EventEmitter;
const Point = Mep.require('misc/geometry/Point');
const MotionDriver = Mep.require('drivers/motion/MotionDriver');
const MotionTargetQueue = require('./MotionTargetQueue');
const Line = Mep.require('misc/geometry/Line');
const Misc = Mep.require('misc/geometry/Misc');
const TAG = 'MotionService';
/**
* Provides a very abstract way to control and estimate robot position
* @fires services.motion.MotionService#pathObstacleDetected
* @memberOf services.position
* @author Darko Lukic <lukicdarkoo@gmail.com>
*/
class MotionService extends EventEmitter {
init(config) {
this.config = Object.assign({
hazardObstacleDistance: 400,
maxBypassTolerance: 50,
targetLineOffset: 150,
hazardAngleFront: [-70, 70],
hazardAngleBack: [110, -110],
epsilonRadius: 30
}, config);
this.motionDriver = Mep.getDriver('MotionDriver');
// Important for simulation
this._targetQueue = new MotionTargetQueue((targets) => {
Mep.Telemetry.send(TAG, 'TargetQueueChanged', targets);
});
Mep.Telemetry.send(TAG, 'HazardObstacleDistanceSet', {
hazardObstacleDistance: this.config.hazardObstacleDistance
});
// Global resolve and reject used outside (strategies)
this._resolve = null;
this._reject = null;
this._paused = false;
this._lastDirection = MotionDriver.DIRECTION_UNDEFINED;
this._obstacleDetectedTimeout = null;
this._pathObstacleDetectedTimeout = null;
// Event method configuration
this._goToNextQueuedTarget = this._goToNextQueuedTarget.bind(this);
this._onObstacleDetected = this._onObstacleDetected.bind(this);
this._onPathObstacleDetected = this._onPathObstacleDetected.bind(this);
// Subscribe on sensors that can provide obstacles on the robot's terrain
Mep.Terrain.on('obstacleDetected', this._onObstacleDetected);
}
isPaused() {
return this._paused;
}
_onObstacleDetected(params) {
if (this._targetQueue.getTargetFront() === null) return;
let hazardAngle = (this.motionDriver.getDirection() === MotionDriver.DIRECTION_FORWARD) ?
this.config.hazardAngleFront : this.config.hazardAngleBack;
// Hazard region
if (Misc.isAngleInRange(hazardAngle[0], hazardAngle[1], params.relativePoi.getAngleFromZero()) &&
params.relativePoi.getDistance(new Point(0, 0)) < this.config.hazardObstacleDistance) {
let friend = Mep.Share.getLastFriendPosition() !== null &&
params.polygon.isPointInside(Mep.Share.getLastFriendPosition());
if (this._obstacleDetectedTimeout !== null) {
clearTimeout(this._obstacleDetectedTimeout);
} else {
this._onPathObstacleDetected(true, friend);
}
this._obstacleDetectedTimeout = setTimeout(() => {
this._obstacleDetectedTimeout = null;
this._onPathObstacleDetected(false, friend);
}, Mep.Config.get('obstacleMaxPeriod') + 100);
} else {
/*
TODO: Try to redesign a path in runtime
if (target.getParams().rerouting === true) {
this.tryRerouting();
}
*/
}
}
_onPathObstacleDetected(detected, friend) {
if (detected === true) {
let target = this._targetQueue.getTargetFront();
let timeout = (friend === true) ? target.getParams().friend : target.getParams().obstacle;
Mep.Motion.stop();
Mep.Log.debug(TAG, 'Obstacle detected, robot stopped');
this._pathObstacleDetectedTimeout = setTimeout(() => {
Mep.Motion.forceReject(friend);
}, timeout);
} else {
if (this._pathObstacleDetectedTimeout !== null) {
clearTimeout(this._pathObstacleDetectedTimeout);
}
Mep.Motion.resume();
}
}
tryRerouting() {
let motionService = this;
let pfTarget = this._targetQueue.getTargetBack();
if (pfTarget !== null) {
// Redesign path
let points = Mep.Terrain.findPath(Mep.Position.getPosition(), pfTarget.getPoint());
if (points.length > 0) {
let params = pfTarget.getParams();
// Reduce tolerance to get better to get better bypass
params.tolerance = (params.tolerance > this.config.maxBypassTolerance) ?
this.config.maxBypassTolerance :
params.tolerance;
// Redesign a path
this._targetQueue.empty();
this._targetQueue.addPointsBack(points, params);
if (params.tolerance === -1) {
this.stop().then(() => {
motionService.resume();
});
} else {
this.motionDriver.finishCommand();
this.resume();
}
} else {
Mep.Log.warn(TAG, 'Cannot redesign path, possible crash!');
// There will be no crash if obstacle move away or
// if robot stop thanks to `pathObstacleDetected` sensors
}
}
}
/**
* Move the robot, set new position of the robot
* @param {TunedPoint} tunedPoint Point that should be reached
* @param {Object} [parameters] Configuration options.
* @param {Boolean} [parameters.pf] Use terrain finding algorithm.
* @param {Boolean} [parameters.backward] Set backward robot moving.
* @param {Boolean} [parameters.rerouting] Enable rerouting during the movement.
* @param {Boolean} [parameters.relative] Use relative to previous position.
* @param {Number} [parameters.tolerance] Position will consider as reached if Euclid's distance between current
* and required position is less than tolerance.
* @param {Number} [parameters.speed] Speed of the robot movement in range (0, 255).
* @param {Number} [parameters.obstacle] Time [ms] after command will be rejected (with TaskError.action === 'obstacle')
* if obstacle is detected in hazard region
* @param {Number} [parameters.friend] Time [ms] after command will be rejected (with TaskError.action === 'friend')
* if friend is detected in hazard region
* @returns {Promise}
*/
go(tunedPoint, parameters) {
let point = tunedPoint.getPoint();
let params = Object.assign({}, this.config.moveOptions, parameters);
// Update last moving direction
if (this.motionDriver.getDirection() !== MotionDriver.DIRECTION_UNDEFINED) {
this._lastDirection = this.motionDriver.getDirection();
}
this._targetQueue.empty();
// Apply relative
if (params.relative === true) {
point.translate(Mep.Position.getPosition());
}
// Apply path finding algorithm
if (params.pf === true) {
let currentPoint = Mep.Position.getPosition();
this._targetQueue.addPointsBack(Mep.Terrain.findPath(currentPoint, point), params);
Mep.Log.debug(TAG, 'Start path finding from', currentPoint, 'to', this._targetQueue.getTargets());
} else {
this._targetQueue.addPointBack(point, params)
}
return new Promise((resolve, reject) => {
if (this._targetQueue.isEmpty()) {
reject(new TaskError(TAG, 'pf', 'Cannot go to required position, no path found'));
return;
}
this._resolve = resolve;
this._reject = reject;
this._goToNextQueuedTarget();
});
}
_goToNextQueuedTarget() {
let motionService = this;
if (this._targetQueue.isEmpty()) {
if (this._resolve !== null) {
this._resolve();
}
} else {
let target = this._targetQueue.getTargetFront();
this._goSingleTarget(target.getPoint(), target.getParams()).then(() => {
motionService._targetQueue.removeFront();
motionService._goToNextQueuedTarget();
}).catch((e) => {
if (e.action !== 'break') {
Mep.Log.error(TAG, e);
motionService._reject(e);
}
});
}
}
/**
* Go to single point without advanced features
* @param {misc.Point} point Target point
* @param {Object} params Additional options
* @param {Boolean} [params.backward] Move robot backward
* @param {Number} [params.tolerance] Max radius
* @param {Number} [params.speed] Speed
* @return {Promise}
* @private
*/
_goSingleTarget(point, params) {
Mep.Log.debug(TAG, 'Simple target go', point);
this._paused = false;
// Set speed
if (params.speed !== undefined && this.motionDriver.getActiveSpeed() !== params.speed) {
this.motionDriver.setSpeed(params.speed);
}
// Move the robot
if (params.tolerance < 0) {
return this.motionDriver.moveToPosition(
point,
params.backward ? -1 : 1
);
} else {
return this.motionDriver.moveToCurvilinear(
point,
params.backward ? -1 : 1,
params.radius,
params.tolerance
);
}
}
/**
* Stop the robot
* @param {Boolean} softStop If true robot will turn of motors
*/
stop(softStop = false) {
this.pause();
if (softStop === true) {
return this.motionDriver.softStop();
} else {
return this.motionDriver.stop();
}
}
pause() {
this._paused = true;
}
resume() {
if (this._paused === true) {
this._paused = false;
this._goToNextQueuedTarget();
}
}
/**
* Move robot forward or backward depending on param `millimeters`
* @param {Number} millimeters Path that needs to be passed. If negative robot will go backward
* @param {Object} params
* @param {Number} [params.speed] Speed
* @params {Boolean} [params.opposite] Move in opposite direction to previous command
* @returns {Promise}
*/
straight(millimeters, params) {
// Update last moving directiObstacle is too long in front of roboton
if (this.motionDriver.getDirection() !== MotionDriver.DIRECTION_UNDEFINED) {
this._lastDirection = this.motionDriver.getDirection();
}
// Apply params
if (params !== undefined) {
// Apply speed
if (params.speed !== undefined && this.motionDriver.getActiveSpeed() !== params.speed) {
this.motionDriver.setSpeed(params.speed);
}
// Apply inverse
if (params.opposite !== undefined) {
switch (this._lastDirection) {
case MotionDriver.DIRECTION_FORWARD:
millimeters = (-1) * Math.abs(millimeters);
break;
case MotionDriver.DIRECTION_BACKWARD:
millimeters = Math.abs(millimeters);
break;
}
}
}
return this.motionDriver.goForward(millimeters);
}
/**
* Rotate robot for an angle
* @param {TunedAngle} tunedAngle Angle to rotate
* @param {Object} options Additional options
* @returns {Promise}
*/
rotate(tunedAngle, options) {
return this.motionDriver.rotateTo(tunedAngle.getAngle());
}
forceReject(friend) {
if (this._reject !== null) {
this._targetQueue.empty();
if (friend === true) {
this._reject(new TaskError(TAG, 'friend', 'Friend is too long in front of robot'));
} else {
this._reject(new TaskError(TAG, 'obstacle', 'Obstacle is too long in front of robot'));
}
}
}
}
module.exports = MotionService;