Colony » Colony Scout

/*

 * This package was developed by the CMU Robotics Club project using code

 * from Willow Garage, Inc.  Please see licensing.txt for details.

 *

 * All rights reserved.

 *

 * @file scout.h

 * @author Colony Project, CMU Robotics Club

 * @author Alex Zirbel

 */

#ifndef SCOUTSIM_SCOUT_H

#define SCOUTSIM_SCOUT_H

#include <ros/ros.h>

#include <boost/shared_ptr.hpp>

#include <motors/set_motors.h>

#include <scoutsim/Pose.h>

#include <scoutsim/Velocity.h>

#include <scoutsim/SetPen.h>

#include <scoutsim/TeleportRelative.h>

#include <scoutsim/TeleportAbsolute.h>

#include <scoutsim/Color.h>

#include <wx/wx.h>

#define PI 3.14159265

namespace scoutsim

{

    struct Vector2

    {

        Vector2()

            : x(0.0)

              , y(0.0)

        {}

        Vector2(float _x, float _y)

            : x(_x)

              , y(_y)

        {}

        bool operator==(const Vector2& rhs)

        {

            return x == rhs.x && y == rhs.y;

        }

        bool operator!=(const Vector2& rhs)

        {

            return x != rhs.x || y != rhs.y;

        }

        float x;

        float y;

    };

    class Scout

    {

        public:

            Scout(const ros::NodeHandle& nh, const wxImage& scout_image,

                  const Vector2& pos, float orient);

            void update(double dt, wxMemoryDC& path_dc,

                        const wxImage& path_image, wxColour background_color,

                        float canvas_width, float canvas_height);

            void paint(wxDC& dc);

        private:

            void setMotors(const motors::set_motors::ConstPtr& msg);

            void velocityCallback(const VelocityConstPtr& vel);

            bool setPenCallback(scoutsim::SetPen::Request&,

                                scoutsim::SetPen::Response&);

            bool teleportRelativeCallback(scoutsim::TeleportRelative::Request&,

                                          scoutsim::TeleportRelative::Response&);

            bool teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request&,

                                          scoutsim::TeleportAbsolute::Response&);

            ros::NodeHandle n_;

            wxImage scout_image_;

            wxBitmap scout_;

            Vector2 pos_;

            float orient_;

            // Keep track of the last commanded speeds sent to the sim

            int motor_fl_speed_;

            int motor_fr_speed_;

            int motor_bl_speed_;

            int motor_br_speed_;

            float lin_vel_;

            float ang_vel_;

            bool pen_on_;

            wxPen pen_;

            ros::Subscriber motors_sub_;

            ros::Subscriber velocity_sub_;

            ros::Publisher pose_pub_;

            ros::Publisher color_pub_;

            ros::ServiceServer set_pen_srv_;

            ros::ServiceServer teleport_relative_srv_;

            ros::ServiceServer teleport_absolute_srv_;

            ros::WallTime last_command_time_;

            float meter_;

            struct TeleportRequest

            {

                TeleportRequest(float x, float y, float _theta,

                                float _linear, bool _relative)

                    : pos(x, y)

                      , theta(_theta)

                      , linear(_linear)

                      , relative(_relative)

                {}

                Vector2 pos;

                float theta;

                float linear;

                bool relative;

            };

            typedef std::vector<TeleportRequest> V_TeleportRequest;

            V_TeleportRequest teleport_requests_;

    };

    typedef boost::shared_ptr<Scout> ScoutPtr;

}

#endif