uwsmposition.h

Go to the documentation of this file.

//
// Copyright (c) 2017 Regents of the SIGNET lab, University of Padova.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Padova (SIGNET lab) nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
 
#ifndef _UWSMPOSITION_
#define _UWSMPOSITION_
 
#include <node-core.h>
 
#define sgn(x) (((x) == 0.0) ? 0.0 : ((x) / fabs(x)))
#define pi (4 * atan(1.0))
 
class UWSMPosition : public Position
{
public:
    UWSMPosition();
 
    virtual ~UWSMPosition() = default;
 
    virtual double getX() override;
 
    virtual double getY() override;
 
    virtual double getZ() override;
 
    virtual double getXdest() const;
 
    virtual double getYdest() const;
 
    virtual double getZdest() const;
 
    double getSpeed() const;
 
    virtual int command(int argc, const char *const *argv) override;
 
    /*
     * Set the destination of the node with a constant speed.
     *
     * @param double Coordinate along x-axis of destination point
     * @param double Coordinate along y-axis of destination point
     * @param double Coordinate along z-axis of destination point
     * @param double Speed of the node
     */
    virtual void setdest(
            double x_dest, double y_dest, double z_dest, double speed);
 
    /*
     * Set the destination of the node with default speed.
     *
     * @param double Coordinate along x-axis of destination point
     * @param double Coordinate along y-axis of destination point
     * @param double Coordinate along z-axis of destination point
     * @param double Speed of the node
     */
    virtual void setdest(double x_dest, double y_dest, double z_dest);
 
    /*
     * Check if the set destination is reached or not.
     *
     * @return bool True if reached, false otherwise
     */
    virtual bool isDestReached() const;
 
    virtual void setX(double x) override;
 
    virtual void setY(double y) override;
 
    virtual void setZ(double z) override;
 
private:
    virtual void update(double now);
 
    int debug_;
    double trgTime_; 
    double lastUpdateTime_; 
    double Xdest_; 
    double Ydest_; 
    double Zdest_; 
    double Xsorg_; 
    double Ysorg_; 
    double Zsorg_; 
    double speed_; 
};
 
#endif