Waypoint
========


This actuator reads the coordinates of a destination point, and moves the robot
towards the given point, with the robot restricted to moving only forward,
turning around its Z axis, and possibly going up and down.
This controller is meant to be used mainly by non-holonomic robots.

While a robot is moving towards a given waypoint, a property of the
**Robot** will be changed in indicate the status of the robot.
The ``movement_status`` property will take one of these values: **Stop**,
**Transit** or **Arrived**.

The movement speed of the robot is internally adjusted to the Blender time
measure, following the formula: ``blender_speed = given_speed * tics``, where
**tics** is the number of times the code gets executed per second.
The default value in Blender is ``tics = 60``.

This actuator also implements a simple obstacle avoidance mechanism. The blend
file contains the **Motion_Controller** empty, as well as two additional Empty
objects: **Radar.L** and **Radar.R**.
These detect nearby objects to the left or right of the robot, and will
instruct the robot to turn in the opposite direction of the obstacle.
If the radar objects are not present, the controller will not have any obstacle
avoidance, and the robot can get blocked by any object between it and the
target.

.. note:: For objects to be detectable by the radars, they must have the
    following settings in the **Physics Properties** panel:

    - **Actor** must be checked
    - **Collision Bounds** must be checked

    This will work even for Static objects


.. cssclass:: properties morse-section

Configuration parameters for waypoint
-------------------------------------


You can set these properties in your scripts with ``<component>.properties(<property1>=..., <property2>=...)``.

- ``ObstacleAvoidance`` (bool, default: ``True``)
	if true (default), will activate obstacle avoidance if the radars are present
- ``FreeZ`` (bool, default: ``False``)
	if false (default), the robot is only controlled on 'X' and heading; if true, 'Z' is also controlled (for aerial or submarine robots)
- ``AngleTolerance`` (float, default: ``0.17453292519943295``)
	Tolerance in radian regarding the final heading of the robot
- ``Speed`` (float, default: ``1.0``)
	movement speed for the robot, given in m/s
- ``Target`` (string, default: ``""``)
	name of a blender object in the scene. When specified, this             object will be placed at the coordinates given to the             actuator, to indicate the expected destination of the             robot
- ``Ignore`` (string, default: ``[]``)
	List of property names. If the object detected by the             radars has any of these properties defined, it will be             ignored by the obstacle avoidance, and will not make the             robot change its trajectory. Useful when trying to move             close to an object of a certain kind
- ``ControlType`` (string, default: ``"Velocity"``)
	Kind of control, can be one of ['Velocity', 'Position']


.. cssclass:: fields morse-section

Data fields
-----------


This actuator reads these datafields at each simulation step:

- ``x`` (float, initial value: ``0.0``)
	X coordinate of the destination, in world frame, in meter
- ``y`` (float, initial value: ``0.0``)
	Y coordinate of the destination, in world frame, in meter
- ``z`` (float, initial value: ``0.0``)
	Z coordinate of the destination, in world frame, in meter
- ``tolerance`` (float, initial value: ``0.5``)
	Tolerance, in meter, to consider the destination as reached.
- ``speed`` (float, initial value: ``1.0``)
	If the property 'Speed' is set, use this speed as initial value.

*Interface support:*

- :tag:`yarp_json`  as yarp::Bottle (:py:mod:`morse.middleware.yarp.yarp_json.YarpJsonReader`)
- :tag:`socket`  as straight JSON deserialization (:py:mod:`morse.middleware.socket_datastream.SocketReader`)
- :tag:`yarp`  as yarp::Bottle (:py:mod:`morse.middleware.yarp_datastream.YarpReader`)


.. cssclass:: services morse-section

Services for Waypoint
---------------------

- ``goto(x, y, z, tolerance, speed)`` (non blocking)
    This method can be used to give a one time instruction to the
    actuator.  When the robot reaches the destination, it will send
    a reply, indicating that the new status of the robot is "Stop".
    
    
  - Parameters

    - ``x``: x coordinate of the destination, in world frame, in meter
    - ``y``: y coordinate of the destination, in world frame, in meter
    - ``z``: z coordinate of the destination, in world frame, in meter
    - ``tolerance``: tolerance, in meter, to consider the                   destination as reached. Optional (default: 0.5 m).
    - ``speed``: speed to join the goal. Optional (default 1m/s)

- ``resume()`` (blocking)
    Restores the speed to the same value as before the last call to
    the **stop** service. The robot will continue to the last
    waypoint specified.
    
- ``get_properties()`` (blocking)
    Returns the properties of a component.
    
    
  - Return value

    a dictionary of the current component's properties  

- ``get_status()`` (blocking)
    Ask the actuator to send a message indicating the current
    movement status of the parent robot. There are three possible
    states: **Transit**, **Arrived** and **Stop**.
    
- ``stop()`` (blocking)
    This method will instruct the robot to set its speed to 0.0, and
    reply immediately. If a **goto** request is ongoing, it will
    remain "pending", as the current destination is not changed.
    
- ``setdest(x, y, z, tolerance, speed)`` (blocking)
    Set a new waypoint and returns.
    
    The robot will try to go to this position, but the service
    caller has no mean to know when the destination is reached
    or if it failed.
    
    In most cases, the asynchronous service 'goto' should be
    prefered.
    
    
  - Parameters

    - ``x``: x coordinate of the destination, in world frame, in meter
    - ``y``: y coordinate of the destination, in world frame, in meter
    - ``z``: z coordinate of the destination, in world frame, in meter
    - ``tolerance``: tolerance, in meter, to consider the                   destination as reached. Optional (default: 0.5 m).
    - ``speed``: speed to join the goal. Optional (default 1m/s)

  - Return value

    Returns always True (if the robot is already moving, the          previous target is replaced by the new one) except if          the destination is already reached. In this case,          returns ``False``. 

- ``get_configurations()`` (blocking)
    Returns the configurations of a component (parsed from the properties).
    
    
  - Return value

    a dictionary of the current component's configurations  



.. cssclass:: examples morse-section

Examples
--------


The following examples show how to use this component in a *Builder* script:

.. code-block:: python


    from morse.builder import *
    
    robot = ATRV()
    
    # creates a new instance of the actuator
    waypoint = Waypoint()

    # place your component at the correct location
    waypoint.translate(<x>, <y>, <z>)
    waypoint.rotate(<rx>, <ry>, <rz>)
    
    robot.append(waypoint)
    
    # define one or several communication interface, like 'socket'
    waypoint.add_interface(<interface>)

    env = Environment('empty')
    

.. cssclass:: files morse-section

Other sources of examples
+++++++++++++++++++++++++

- `Source code <../../_modules/morse/actuators/waypoint.html>`_
- `Unit-test <../../_modules/base/waypoint_testing.html>`_




*(This page has been auto-generated from MORSE module morse.actuators.waypoint.)*
