position.h

Go to the documentation of this file.

#ifndef POSITION_H
#define POSITION_H

#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/Pose.h>
#include <cpswarm_msgs/OutOfBounds.h>
#include <cpswarm_msgs/GetSector.h>

using namespace std;
using namespace ros;

class position
{
public:
    position ();

    ~position ();

    double bearing (geometry_msgs::Pose p) const;

    geometry_msgs::Pose compute_goal (double distance, double direction) const;

    geometry_msgs::Pose compute_goal (geometry_msgs::Pose start, double distance, double direction) const;

    double dist (geometry_msgs::Pose p) const;

    double dist (geometry_msgs::Pose p1, geometry_msgs::Pose p2) const;

    geometry_msgs::Pose get_pose () const;

    double get_yaw () const;

    bool move (geometry_msgs::Pose pose);

    bool occupied (geometry_msgs::Pose pose);

    bool out_of_bounds (geometry_msgs::Pose pose);

private:
    double get_yaw (geometry_msgs::Pose pose) const;

    bool reached (geometry_msgs::Pose goal);

    void pose_callback (const geometry_msgs::PoseStamped::ConstPtr& msg);

    Subscriber pose_sub;

    ServiceClient out_of_bounds_client;

    ServiceClient occupied_sector_client;

    Publisher pose_pub;

    NodeHandle nh;

    Rate* rate;

    geometry_msgs::Pose pose;

    bool pose_valid;

    double goal_timeout;

    double goal_tolerance;
};

#endif // POSITION_H