no_collision_alg.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _no_collision_alg_h_
#define _no_collision_alg_h_

#include <iri_no_collision/NoCollisionConfig.h>
#include "mutex.h"

//include no_collision_alg main library
#include <geometry_msgs/Point.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

typedef enum {traj_acc,traj_speed,traj_deacc,traj_idle} TrajStates;

class NoCollisionAlgorithm
{
  protected:
    CMutex alg_mutex_;

    // private attributes and methods
    static const float MIN_LASER_RANGE_ = 0.005f; //[m]
    // node configuration attributes
    float MIN_SAFE_DISTANCE_; // [m]
    float LASER_SAFE_DIST_; // [m]
    float VT_MAX_; // [m/s]
    float T_ACC_; // [m/s2]
    float ANGLE_TOL_; // [rad]

    float MIN_SAFE_ANGLE_; // [rad]
    float VR_MAX_; // [rad/s]
    float R_ACC_; // [rad/s2]
    float DIST_TOL_; // [meters]
  
    float OA_CONE_ANGLE_; // [rad]

    float MAX_ANGLE_TO_STOP_; //[rad]

    // translational parameters
    float current_vt_; // [m/s]
    float target_vt_;//[m/s]
    float desired_vt_;//[m/s]
    float acc_dist_;//[m]
    float deacc_dist_;//[m]
    float target_dist_;//[m]
    float current_dist_;//[m]
    TrajStates vt_state_;

    // rotational parameters
    float current_vr_; // [rad/s]
    float target_vr_;//[rad/s]
    float desired_vr_;//[rad/s]
    float acc_angle_;//[rad]
    float deacc_angle_;//[rad]
    float target_angle_;//rad]
    float current_angle_;//[rad]
    TrajStates vr_state_;
    float heading;//[rad]

    ros::Time current_time_;
    void fromCartesian2Polar(const geometry_msgs::Point & p, float & module, float & angle);
                                 
    void fromPolar2Cartesian(const float & module, const float & angle, geometry_msgs::Point &p);

    void updateTranslationTrajParams(void);

    void updateRotationTrajParams(void);
  public:
    typedef iri_no_collision::NoCollisionConfig Config;

    Config config_;

    NoCollisionAlgorithm(void);

    void lock(void) { alg_mutex_.enter(); };

    void unlock(void) { alg_mutex_.exit(); };

    bool try_enter(void) { return alg_mutex_.try_enter(); };

    void config_update(const Config& new_cfg, uint32_t level=0);

    // here define all no_collision_alg interface methods to retrieve and set
    // the driver parameters
    void setGoal(const geometry_msgs::Pose & local_goal);

    bool isGoalReached(void);

    void getDistance2Goal(geometry_msgs::Pose &current_goal);

    void setTranslationalSpeed(const double vt);

    void setRotationalSpeed(const double vr);

    bool movePlatform(const sensor_msgs::LaserScan & laser,const geometry_msgs::Pose & local_goal,geometry_msgs::Twist &twist);

    bool isGoalTraversable(const sensor_msgs::LaserScan & laser, 
                           const geometry_msgs::Point & goal);
  
    ~NoCollisionAlgorithm(void);
};

#endif