Colony » Colony Scout » Colony Scout OS

/*

 * 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/SetPen.h>

#include <scoutsim/TeleportRelative.h>

#include <scoutsim/TeleportAbsolute.h>

#include <scoutsim/Color.h>

#include <wx/wx.h>

#define PI 3.14159265

/// The scale factor so the speed of scout robots is reasonable for the sim

#define SPEED_SCALE_FACTOR 0.05

namespace scoutsim

{

    struct Vector2

    {

        Vector2()

            : x(0.0)

              , y(0.0)

        {}

        Vector2(float new_x, float new_y)

            : x(new_x)

              , y(new_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);

            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 node;

            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::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 new_theta,

                                float new_linear, bool new_relative)

                    : pos(x, y)

                      , theta(new_theta)

                      , linear(new_linear)

                      , relative(new_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