Exemple #1
0
static Geometry2d::Point fromOursToTheirs(Geometry2d::Point& pt) {
    Geometry2d::Point c;
    c.y() = Field_Dimensions::Current_Dimensions.Length() - pt.y();
    c.x() = -pt.x();

    return c;
}
 /** saturates the magnitude of a vector */
 static Geometry2d::Point saturate(Geometry2d::Point value, float max) {
     float mag = value.mag();
     if (mag > fabs(max)) {
         return value.normalized() * fabs(max);
     }
     return value;
 }
Exemple #3
0
float Planning::Path::length(const Geometry2d::Point &pt) const
{
	float dist = -1;
	float length = 0;
	if (points.empty())
	{
		return 0;
	}
	
	for (unsigned int i = 0; i < (points.size() - 1); ++i)
    {
		Geometry2d::Segment s(points[i], points[i+1]);
				
		//add the segment length
		length += s.length();
		
		const float d = s.distTo(pt);
		
		//if point closer to this segment
		if (dist < 0 || d < dist)
		{
			//closest point on segment
			Geometry2d::Point p = s.nearestPoint(pt);
			
			//new best distance
			dist = d;
			
			//reset running length count
			length = 0;
			length += p.distTo(s.pt[1]);
		}
	}
	
	return length;
}
bool Gameplay::Plays::DemoAttack::run()
{
	Geometry2d::Point ballPos = ball().pos;

	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_kicker.robot, available, ballPos);
	// if we have kicked, we want to reset
	if (_kicker.done() &&  ball().valid &&
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_kicker.restart();
	}

	// set flags from parameters
	if (_calculateChipPower->value())
		_kicker.calculateChipPower(ball().pos.distTo(_kicker.target().center()));

	_kicker.use_chipper = *_useChip;
	_kicker.minChipRange = *_minChipRange;
	_kicker.maxChipRange = *_maxChipRange;
	_kicker.dribbler_speed = *_dribblerSpeed;
	_kicker.kick_power = *_kickPower;
	_kicker.use_line_kick = *_useLineKick;

	_kicker.run();

	return true;
}
void SimFieldView::drawTeamSpace(QPainter& p) {
    FieldView::drawTeamSpace(p);

    // Simulator drag-to-shoot
    std::shared_ptr<LogFrame> frame = currentFrame();
    if (_dragMode == DRAG_SHOOT && frame) {
        p.setPen(QPen(Qt::white, 0.1f));
        Geometry2d::Point ball = frame->ball().pos();
        p.drawLine(ball.toQPointF(), _dragTo.toQPointF());

        if (ball != _dragTo) {
            p.setPen(QPen(Qt::gray, 0.1f));

            _shot = (ball - _dragTo) * ShootScale;
            float speed = _shot.mag();
            Geometry2d::Point shotExtension = ball + _shot / speed * 8;

            p.drawLine(ball.toQPointF(), shotExtension.toQPointF());

            p.setPen(Qt::black);
            drawText(p, _dragTo.toQPointF(),
                     QString("%1 m/s").arg(speed, 0, 'f', 1));
        }
    }
}
bool Environment::occluded(Geometry2d::Point ball, Geometry2d::Point camera) {
    float camZ = 4;
    float ballZ = Ball_Radius;
    float intZ = Robot_Height;

    // FIXME: use actual physics engine to determine line of sight

    // Find where the line from the camera to the ball intersects the
    // plane at the top of the robots.
    //
    // intZ = (ballZ - camZ) * t + camZ
    float t = (intZ - camZ) / (ballZ - camZ);
    Geometry2d::Point intersection;
    intersection.x = (ball.x - camera.x) * t + camera.x;
    intersection.y = (ball.y - camera.y) * t + camera.y;

    // Return true if the intersection point is inside any robot
    for (const Robot* r : _blue) {
        if (intersection.nearPoint(r->getPosition(), Robot_Radius)) {
            return true;
        }
    }
    for (const Robot* r : _yellow) {
        if (intersection.nearPoint(r->getPosition(), Robot_Radius)) {
            return true;
        }
    }
    return false;
}
void Environment::sendVision() {
    SSL_WrapperPacket wrapper;
    SSL_DetectionFrame* det = wrapper.mutable_detection();
    det->set_frame_number(_frameNumber++);
    det->set_camera_id(0);

    struct timeval tv;
    gettimeofday(&tv, nullptr);
    det->set_t_capture(tv.tv_sec + (double)tv.tv_usec * 1.0e-6);
    det->set_t_sent(det->t_capture());

    for (Robot* robot : _yellow) {
        if ((rand() % 100) < robot->visibility) {
            SSL_DetectionRobot* out = det->add_robots_yellow();
            convert_robot(robot, out);
        }
    }

    for (Robot* robot : _blue) {
        if ((rand() % 100) < robot->visibility) {
            SSL_DetectionRobot* out = det->add_robots_blue();
            convert_robot(robot, out);
        }
    }

    for (const Ball* b : _balls) {
        Geometry2d::Point ballPos = b->getPosition();

        // bool occ;
        // if (ballPos.x < 0)
        // {
        // 	occ = occluded(ballPos, cam0);
        // } else {
        // 	occ = occluded(ballPos, cam1);
        // }

        if ((rand() % 100) < ballVisibility) {
            SSL_DetectionBall* out = det->add_balls();
            out->set_confidence(1);
            out->set_x(ballPos.x() * 1000);
            out->set_y(ballPos.y() * 1000);
            out->set_pixel_x(ballPos.x() * 1000);
            out->set_pixel_y(ballPos.y() * 1000);
        }
    }

    std::string buf;
    wrapper.SerializeToString(&buf);

    if (sendShared) {
        _visionSocket.writeDatagram(&buf[0], buf.size(), MulticastAddress,
                                    SharedVisionPort);
    } else {
        _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
                                    SimVisionPort);
        _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
                                    SimVisionPort + 1);
    }
}
void Environment::addBall(Geometry2d::Point pos) {
    Ball* b = new Ball(this);
    b->initPhysics();
    b->position(pos.x(), pos.y());

    _balls.append(b);

    printf("New Ball: %f %f\n", pos.x(), pos.y());
}
Geometry2d::Point gaussianPoint(int n, float scale) {
    Geometry2d::Point pt;
    for (int i = 0; i < n; ++i) {
        pt.x() += drand48() - 0.5;
        pt.y() += drand48() - 0.5;
    }
    pt *= scale / n;

    return pt;
}
void Environment::convert_robot(const Robot* robot, SSL_DetectionRobot* out) {
    Geometry2d::Point pos = robot->getPosition();
    out->set_confidence(1);
    out->set_robot_id(robot->shell);
    out->set_x(pos.x() * 1000);
    out->set_y(pos.y() * 1000);
    out->set_orientation(robot->getAngle());
    out->set_pixel_x(pos.x() * 1000);
    out->set_pixel_y(pos.y() * 1000);
}
void OurRobot::pivot(Geometry2d::Point pivotTarget) {
    _rotationCommand = std::make_unique<Planning::EmptyAngleCommand>();

    const float radius = Robot_Radius * 1;
    Geometry2d::Point pivotPoint = _state->ball.pos;

    // reset other conflicting motion commands
    _motionCommand = std::make_unique<Planning::PivotCommand>(
        pivotPoint, pivotTarget, radius);

    *_cmdText << "pivot(" << pivotTarget.x() << ", " << pivotTarget.y() << ")"
              << endl;
}
void OurRobot::moveDirect(Geometry2d::Point goal, float endSpeed) {
    if (!visible) return;

    // sets flags for future movement
    if (verbose)
        cout << " in OurRobot::moveDirect(goal): adding a goal (" << goal.x()
             << ", " << goal.y() << ")" << endl;

    _motionCommand = std::make_unique<Planning::DirectPathTargetCommand>(
        MotionInstant(goal, (goal - pos).normalized() * endSpeed));

    *_cmdText << "moveDirect(" << goal << ")" << endl;
    *_cmdText << "endSpeed(" << endSpeed << ")" << endl;
}
Exemple #13
0
//// Fixed Step Tree ////
Tree::Point* FixedStepTree::extend(Geometry2d::Point pt, Tree::Point* base) {
    // if we don't have a base point, try to find a close point
    if (!base) {
        base = nearest(pt);
        if (!base) {
            return nullptr;
        }
    }

    Geometry2d::Point delta = pt - base->pos;
    float d = delta.mag();

    Geometry2d::Point pos;
    if (d < step) {
        pos = pt;
    } else {
        pos = base->pos + delta / d * step;
    }

    // Check for obstacles.

    // moveHit is the set of obstacles that this move touches.
    // If this move touches any obstacles that the starting point didn't already
    // touch,
    // it has entered an obstacle and will be rejected.
    std::set<shared_ptr<Geometry2d::Shape>> moveHit =
        _obstacles->hitSet(Geometry2d::Segment(pos, base->pos));
    if (!moveHit.empty()) {
        // We only care if there are any items in moveHit that are not in
        // point->hit, so
        // we don't store the result of set_difference.
        try {
            set_difference(
                moveHit.begin(), moveHit.end(), base->hit.begin(),
                base->hit.end(),
                ExceptionIterator<std::shared_ptr<Geometry2d::Shape>>());
        } catch (exception& e) {
            // We hit a new obstacle
            return nullptr;
        }
    }

    // Allow this point to be added to the tree
    Point* p = new Point(pos, base);
    p->hit = std::move(moveHit);
    points.push_back(p);

    return p;
}
void SimFieldView::mousePressEvent(QMouseEvent* me) {
    Geometry2d::Point pos = _worldToTeam * _screenToWorld * me->pos();

    std::shared_ptr<LogFrame> frame = currentFrame();
    if (me->button() == Qt::LeftButton && frame) {
        _dragRobot = -1;
        for (const LogFrame::Robot& r : frame->self()) {
            if (pos.nearPoint(r.pos(), Robot_Radius)) {
                _dragRobot = r.shell();
                _dragRobotBlue = frame->blue_team();
                break;
            }
        }
        for (const LogFrame::Robot& r : frame->opp()) {
            if (pos.nearPoint(r.pos(), Robot_Radius)) {
                _dragRobot = r.shell();
                _dragRobotBlue = !frame->blue_team();
                break;
            }
        }

        if (_dragRobot < 0) {
            placeBall(me->pos());
        }

        _dragMode = DRAG_PLACE;
    } else if (me->button() == Qt::RightButton && frame) {
        if (frame->has_ball() &&
            pos.nearPoint(frame->ball().pos(), Ball_Radius)) {
            // Drag to shoot the ball
            _dragMode = DRAG_SHOOT;
            _dragTo = pos;
        } else {
            // Look for a robot selection
            int newID = -1;
            for (int i = 0; i < frame->self_size(); ++i) {
                if (pos.distTo(frame->self(i).pos()) < Robot_Radius) {
                    newID = frame->self(i).shell();
                    break;
                }
            }

            if (newID != frame->manual_id()) {
                robotSelected(newID);
            }
        }
    }
}
Exemple #15
0
bool Gameplay::Behaviors::Idle::run()
{
	if (!ball().valid || robots.empty())
	{
		// If we can't find the ball, leave the robots where they are
		return false;
	}
	
	// Arrange all robots close together around the ball
	
	// We really should exclude parts of the circle that aren't accessible due to field or rules,
	// but I think it doesn't matter when there are only four robots.
	
	// Radius of the circle that will contain the robot centers
	float radius = Field_CenterRadius + Robot_Radius + .01;
	
	// Angle between robots, as seen from the ball
	float perRobot = (Robot_Diameter * 1.25) / radius * RadiansToDegrees;
	
	// Direction from the ball to the first robot.
	// Center the robots around the line from the ball to our goal
	Geometry2d::Point dir = (Geometry2d::Point() - ball().pos).normalized() * radius;
	dir.rotate(Geometry2d::Point(), -perRobot * (robots.size() - 1) / 2);
	
	state()->drawLine(ball().pos, Geometry2d::Point());
	
	for (OurRobot *r :  robots)
	{
		if (r->visible)
		{
			state()->drawLine(ball().pos, ball().pos + dir);
			
			r->move(ball().pos + dir);
			r->face(ball().pos);
			
			// Avoid the ball even on our restart
			r->avoidBallRadius(Field_CenterRadius);
			// Don't leave gaps
			r->avoidAllTeammates(true);
		}
		
		// Move to the next robot's position
		dir.rotate(Geometry2d::Point(), perRobot);
	}
	
	return false;
}
void Gameplay::WindowEvaluator::obstacleRobot(Geometry2d::Point pos)
{
	Geometry2d::Point n = (pos - _origin).normalized();
	Geometry2d::Point t = n.perpCCW();
	const float r = Robot_Radius + Ball_Radius;
	Geometry2d::Segment seg(pos - n * Robot_Radius + t * r,
							pos - n * Robot_Radius - t * r);
	
	if (debug)
	{
//		_state->debugLines.push_back(seg);
	}
	Geometry2d::Point intersection[2];
	float extent[2] = {0, _end};
	bool valid[2] = {false, false};
	
	for (int i = 0; i < 2; ++i)
	{
		Geometry2d::Line edge(_origin, seg.pt[i]);
		float d = edge.delta().magsq();
		if (edge.intersects(_target, &intersection[i]) && (intersection[i] - _origin).dot(edge.delta()) > d)
		{
			valid[i] = true;
			float t = (intersection[i] - _target.pt[0]).dot(_target.delta());
			
			if (t < 0)
			{
				extent[i] = 0;
			} else if (t > _end)
			{
				extent[i] = _end;
			} else {
				extent[i] = t;
			}
		}
	}
	
	if (!valid[0] || !valid[1])
	{
		// Obstacle has no effect
		return;
	}
	
	obstacleRange(extent[0], extent[1]);
}
Exemple #17
0
bool Gameplay::Plays::DemoBump::run()
{
	Geometry2d::Point ballPos = ball().pos;

	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_bump.robot, available, ballPos);

	// if we have kicked, we want to reset
	if (_bump.done() &&  ball().valid &&
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_bump.restart();
	}

	_bump.run();

	return true;
}
Exemple #18
0
Robot::Robot(Environment* env, unsigned int id, Robot::RobotRevision rev,
             Geometry2d::Point startPos)
    : Entity(env),
      shell(id),
      _rev(rev),
      _robotChassis(nullptr),
      _robotVehicle(nullptr),
      _wheelShape(nullptr),
      _controller(nullptr),
      _brakingForce(0),
      _targetVel(0, 0, 0),
      _targetRot(0),
      _simEngine(env->getSimEngine()) {
    visibility = 100;

    _startTransform.setIdentity();
    _startTransform.setOrigin(btVector3(startPos.y(), 0, startPos.x()) *
                              scaling);
    setEngineForce(0);
}
bool Gameplay::Plays::DemoTouchKick::run()
{
	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_kicker.robot, available, Geometry2d::Point());
	assignNearest(_passer.robot, available, Geometry2d::Point());

	Geometry2d::Point ballPos = ball().pos;

	_passer.setTarget(_kicker.robot->kickerBar());
	_passer.use_line_kick = true;

	// if we have kicked, we want to reset
	if (_kicker.done() &&  ball().valid && 
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_kicker.restart();
		_passer.restart();
	}
	
	if (_passer.done() && ball().valid)
	{

	}

	if (_kicker.robot->pos.distTo(_passTarget) < 0.5)
	{
		_kicker.run();
	}
	else
	{
		_kicker.robot->move(_passTarget);
	}

	_kicker.use_chipper = *_use_chipper;
	_kicker.kick_power = *_kick_power;

	_passer.run();

	return true;
}
Exemple #20
0
// Returns the index of the point in this path nearest to pt.
int Planning::Path::nearestIndex(const Geometry2d::Point &pt) const
{
	if (points.size() == 0)
	{
		return -1;
	}
	
	int index = 0;
	float dist = pt.distTo(points[0]);
	
	for (unsigned int i=1 ; i<points.size(); ++i)
	{
		float d = pt.distTo(points[i]);
		if (d < dist)
		{
			dist = d;
			index = i;
		}
	}
	
	return index;
}
bool Gameplay::Plays::DemoYank::run()
{
	Geometry2d::Point ballPos = ball().pos;

	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_yank.robot, available, ballPos);

	// if we have kicked, we want to reset
	if (_yank.done() &&  ball().valid &&
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_yank.restart();
	}

	// set flags from parameters
	_yank.dribble_speed = *_dribblerSpeed;
	_yank.enable_bump = *_enableBump;

	_yank.run();

	return true;
}
 Geometry2d::ShapeSet createRobotObstacles(const std::vector<ROBOT*>& robots,
                                           const RobotMask& mask,
                                           Geometry2d::Point currentPosition,
                                           float checkRadius) const {
     Geometry2d::ShapeSet result;
     for (size_t i = 0; i < mask.size(); ++i)
         if (mask[i] > 0 && robots[i] && robots[i]->visible) {
             if (currentPosition.distTo(robots[i]->pos) <= checkRadius) {
                 result.add(std::shared_ptr<Geometry2d::Shape>(
                     new Geometry2d::Circle(robots[i]->pos, mask[i])));
             }
         }
     return result;
 }
TEST(WorldRobot, one_robot) {
    RJ::Time t = RJ::now();
    Geometry2d::Point p = Geometry2d::Point(1,1);
    double th = 1;
    int rID = 1;
    CameraRobot b = CameraRobot(t, p, th, rID);
    int cID = 1;
    WorldRobot w;

    KalmanRobot kb = KalmanRobot(cID, t, b, w);

    std::list<KalmanRobot> kbl;
    kbl.push_back(kb);

    WorldRobot wb = WorldRobot(t, WorldRobot::Team::BLUE, rID, kbl);

    Geometry2d::Point rp = wb.getPos();
    double rt = wb.getTheta();
    Geometry2d::Point rv = wb.getVel();
    double ro = wb.getOmega();
    
    double rpc = wb.getPosCov();
    double rvc = wb.getVelCov();

    std::list<KalmanRobot> list = wb.getRobotComponents();

    EXPECT_TRUE(wb.getIsValid());
    EXPECT_EQ(wb.getRobotID(), rID);
    EXPECT_EQ(rp.x(), p.x());
    EXPECT_EQ(rp.y(), p.y());
    EXPECT_EQ(rt, th);
    EXPECT_EQ(rv.x(), 0);
    EXPECT_EQ(rv.y(), 0);
    EXPECT_EQ(ro, 0);
    EXPECT_GT(rpc, 0);
    EXPECT_GT(rvc, 0);
    EXPECT_LT(rpc, 10000);
    EXPECT_LT(rvc, 10000);
    EXPECT_EQ(list.size(), 1);
}
Exemple #24
0
bool Circle::tangentPoints(const Geometry2d::Point &src, 
	Geometry2d::Point* p1, Geometry2d::Point* p2) const
{
	if (!p1 && !p2)
	{
		return false;
	}
	
	const float dist = src.distTo(center);
	
	if (dist < _r)
	{
		return false;
	}
	else if (dist == _r)
	{
		if (p1)
		{
			*p1 = src;
		}
		
		if (p2)
		{
			*p2 = src;
		}
	}
	else
	{
		//source is outside of circle
		const float theta = asin(_r/dist);
		const float degT = theta * 180.0f / M_PI;
		
		if (p1)
		{
			Point final = center;
			final.rotate(src, degT);
			*p1 = final;
		}
		
		if (p2)
		{
			Point final = center;
			final.rotate(src, -degT);
			*p2 = final;
		}
void OurRobot::move(Geometry2d::Point goal, Geometry2d::Point endVelocity) {
    if (!visible) return;

    // sets flags for future movement
    if (verbose)
        cout << " in OurRobot::move(goal): adding a goal (" << goal.x() << ", "
             << goal.y() << ")" << std::endl;

    _motionCommand = std::make_unique<Planning::PathTargetCommand>(
        MotionInstant(goal, endVelocity));

    *_cmdText << "move(" << goal.x() << ", " << goal.y() << ")" << endl;
    *_cmdText << "endVelocity(" << endVelocity.x() << ", " << endVelocity.y()
              << ")" << endl;
}
TEST(WorldRobot, two_robot) {
    RJ::Time t = RJ::now();
    Geometry2d::Point p1 = Geometry2d::Point(1,1);
    Geometry2d::Point p2 = Geometry2d::Point(2,2);
    double th1 = 1;
    double th2 = 2;
    int rID = 1;
    CameraRobot b1 = CameraRobot(t, p1, th1, rID);
    CameraRobot b2 = CameraRobot(t, p2, th2, rID);
    int cID = 1;
    WorldRobot w;

    KalmanRobot kb1 = KalmanRobot(cID, t, b1, w);
    KalmanRobot kb2 = KalmanRobot(cID, t, b2, w);

    std::list<KalmanRobot> kbl;
    kbl.push_back(kb1);
    kbl.push_back(kb2);

    WorldRobot wb = WorldRobot(t, WorldRobot::Team::BLUE, rID, kbl);

    Geometry2d::Point rp = wb.getPos();
    double rt = wb.getTheta();
    Geometry2d::Point rv = wb.getVel();
    double ro = wb.getOmega();
    double rpc = wb.getPosCov();
    double rvc = wb.getVelCov();

    std::list<KalmanRobot> list = wb.getRobotComponents();

    EXPECT_TRUE(wb.getIsValid());
    EXPECT_EQ(rp.x(), (p1.x() + p2.x()) / 2);
    EXPECT_EQ(rp.y(), (p1.y() + p2.y()) / 2);
    EXPECT_EQ(rt, (th1 + th2) / 2);
    EXPECT_EQ(rv.x(), 0);
    EXPECT_EQ(rv.y(), 0);
    EXPECT_EQ(ro, 0);
    EXPECT_GT(rpc, 0);
    EXPECT_GT(rvc, 0);
    EXPECT_LT(rpc, 10000);
    EXPECT_LT(rvc, 10000);
    EXPECT_EQ(list.size(), 2);
}
Exemple #27
0
/** Checks whether or not the given ball is in the defense area. */
static inline bool ballIsInGoalieBox(Geometry2d::Point point) {
    if (std::abs(point.x()) <
        Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f) {
        // Ball is in center (rectangular) portion of defensive bubble
        return point.y() > 0 &&
               point.y() < Field_Dimensions::Current_Dimensions.ArcRadius();
    } else if (std::abs(point.x()) <
               (Field_Dimensions::Current_Dimensions.ArcRadius() +
                Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f)) {
        // Ball is in one of the side (arc) portions of defensive bubble
        double adjusted_x =
            std::abs(point.x()) -
            (Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f);
        double max_y = sqrt((Field_Dimensions::Current_Dimensions.ArcRadius() *
                             Field_Dimensions::Current_Dimensions.ArcRadius()) -
                            (adjusted_x * adjusted_x));
        return point.y() > 0 && point.y() <= max_y;
    }
    return false;
}
void Environment::addRobot(bool blue, int id, Geometry2d::Point pos,
                           Robot::RobotRevision rev) {
    Robot* r = new Robot(this, id, rev, pos);
    r->initPhysics(blue);

    if (blue) {
        _blue.insert(id, r);
    } else {
        _yellow.insert(id, r);
    }

    Geometry2d::Point actPos = r->getPosition();

    switch (rev) {
        case Robot::rev2008:
            printf("New 2008 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
            break;
        case Robot::rev2011:
            printf("New 2011 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
    }

    QVector3D pos3(pos.x(), pos.y(), 0.0);
    QVector3D axis(0.0, 0.0, 1.0);
}
void OurRobot::face(Geometry2d::Point pt) {
    _rotationCommand = std::make_unique<Planning::FacePointCommand>(pt);

    *_cmdText << "face(" << pt.x() << ", " << pt.y() << ")" << endl;
}
void OurRobot::worldVelocity(Geometry2d::Point v) {
    _motionCommand = std::make_unique<Planning::WorldVelTargetCommand>(v);
    setPath(nullptr);
    *_cmdText << "worldVel(" << v.x() << ", " << v.y() << ")" << endl;
}