Colony » Colony Scout » Colony Scout OS

*~

/**

 ******************************************************************************

 * WARNING: THIS IS AN AUTOGENERATED FILE!

 *

 * Editing this file is USELESS. It will be overwritten during the build.

 * To properly edit this file, edit its corresponding *.template.* file, which

 * a script run by CMake uses to generate this file.

 *

 * This file was generated via rules in src/generate_behavior_lists.py.

 *

 * Please see that file for a description of syntax and procedure for

 * creating auto-generated files.

 *

 * THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION!

 * THANK YOU.

 *****************************************************************************

 */

#include "BehaviorList.h"

BehaviorList::BehaviorList()

{

  behavior_list.push_back(behavior<pause_scout>);

  behavior_list.push_back(behavior<danger_marking>);

  behavior_list.push_back(behavior<draw_ccw_circle>);

  behavior_list.push_back(behavior<draw_cw_circle>);

  behavior_list.push_back(behavior<line_follow>);

  behavior_list.push_back(behavior<maze_solve>);

  behavior_list.push_back(behavior<maze_solve_simple>);

  behavior_list.push_back(behavior<navigationMap>);

  behavior_list.push_back(behavior<Odometry>);

  behavior_list.push_back(behavior<Odometry_new>);

  behavior_list.push_back(behavior<Scheduler>);

  behavior_list.push_back(behavior<smart_runaround>);

  behavior_list.push_back(behavior<WH_Robot>);

  behavior_list.push_back(behavior<wl_test>);

  return;

}

BehaviorList::~BehaviorList()

{

  while(!behavior_list.empty())

    behavior_list.pop_back();

}

/**

 BETA version of Emitter object for use in BOM simulator

 */

#include "emitter.h"

#include <wx/wx.h>

#define DEFAULT_PEN_R 0xb3

#define DEFAULT_PEN_G 0xb8

#define DEFAULT_PEN_B 0xff

using namespace std;

namespace scoutsim

{

    /**

     * The emitter object, which is responsible for refreshing itself and

     * updating its position.

     *

     */

    Emitter::Emitter(const ros::NodeHandle& nh,

                 const wxImage& emitter_image,

                 const Vector2& pos,

                 wxBitmap *path_bitmap,

                 float orient,

                 float aperture, 

                 int distance)

        : path_bitmap(path_bitmap)

          , ignore_behavior(false)

          , node (nh)

          , emitter_image(emitter_image)

          , pos(pos)

          , orient(orient)

          , aperture(aperture)

          , distance(distance)

          , pen_on(true)

          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))

    {

        pen.SetWidth(3);

        emitter = wxBitmap(emitter_image);

        color_pub = node.advertise<Color>("color_sensor", 1);

        set_pen_srv = node.advertiseService("set_pen",

                                            &Emitter::setPenCallback,

                                            this);

        // Init latch

        teleop_latch = 0;

    }

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

                               scoutsim::SetPen::Response&)

    {

        pen_on = !req.off;

        if (req.off)

        {

            return true;

        }

        wxPen pen(wxColour(req.r, req.g, req.b));

        if (req.width != 0)

        {

            pen.SetWidth(req.width);

        }

        pen = pen;

        return true;

    }

    // Scale to linesensor value

    unsigned int Emitter::rgb_to_grey(unsigned char r,

                                    unsigned char g,

                                    unsigned char b)

    {

        // Should be 0 to 255

        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;

        /// @todo Convert to the proper range

        return 255 - grey;

    }

    /// Sends back the position of this emitter so scoutsim can save

    /// the world state

    /// @todo remove dt param

    geometry_msgs::Pose2D Emitter::update(double dt, wxMemoryDC& path_dc,

                                        const wxImage& path_image,

                                        const wxImage& lines_image,

                                        const wxImage& walls_image,

                                        wxColour background_color,

                                        world_state state)

    {

        pos.x = min(max(pos.x, 0.0f), state.canvas_width);

        pos.y = min(max(pos.y, 0.0f), state.canvas_height);

        //int canvas_x = pos.x * PIX_PER_METER;

        //int canvas_y = pos.y * PIX_PER_METER;

        {

            wxImage rotated_image =

                emitter_image.Rotate(orient - PI/2.0,

                                   wxPoint(emitter_image.GetWidth() / 2,

                                           emitter_image.GetHeight() / 2),

                                   false);

            for (int y = 0; y < rotated_image.GetHeight(); ++y)

            {

                for (int x = 0; x < rotated_image.GetWidth(); ++x)

                {

                    if (rotated_image.GetRed(x, y) == 255

                            && rotated_image.GetBlue(x, y) == 255

                            && rotated_image.GetGreen(x, y) == 255)

                    {

                        rotated_image.SetAlpha(x, y, 0);

                    }

                }

            }

            emitter = wxBitmap(rotated_image);

        }

        geometry_msgs::Pose2D my_pose;

        my_pose.x = pos.x;

        my_pose.y = pos.y;

        my_pose.theta = orient;

        return my_pose;

    }

    geometry_msgs::Pose2D Emitter::get_pos()

    {

        geometry_msgs::Pose2D my_pose;

        my_pose.x = pos.x;

        my_pose.y = pos.y;

        my_pose.theta = orient;

        return my_pose;

    }

    void Emitter::paint(wxDC& dc)

    {

        wxSize emitter_size = wxSize(emitter.GetWidth(), emitter.GetHeight());

        dc.DrawBitmap(emitter, pos.x * PIX_PER_METER - (emitter_size.GetWidth() / 2),

                      pos.y * PIX_PER_METER - (emitter_size.GetHeight() / 2), true);

    }

    void Emitter::set_emitter_visual(bool on)

    {

        emitter_visual_on = on;

    }

}

/** @} */

#ifndef _SCOUTSIM_EMITTER_H_

#define _SCOUTSIM_EMITTER_H_

#include <ros/ros.h>

#include <vector>

#include <boost/shared_ptr.hpp>

#include <scoutsim/Pose.h>

#include <scoutsim/SetPen.h>

#include <scoutsim/Color.h>

#include <geometry_msgs/Pose2D.h>

#include <wx/wx.h>

#include "scout.h"

#include "scoutsim_internal.h"

#include "scout_constants.h"

#define PI 3.14159265

#define NUM_LINESENSORS 8

// Distance, pixels, from center of robot to the linesensors.

#define LNSNSR_D 20

namespace scoutsim

{

    class Emitter

    {

        public:

            Emitter(const ros::NodeHandle& nh,const wxImage& emitter_image,

                  const Vector2& pos, wxBitmap *path_bitmap, float orient,

                  float aperture, int distance);

            geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,

                                         const wxImage& path_image,

                                         const wxImage& lines_image,

                                         const wxImage& walls_image,

                                         wxColour background_color,

                                         world_state state);

            geometry_msgs::Pose2D get_pos();

            void paint(wxDC& dc);

            void set_emitter_visual(bool on);

        private:

            float absolute_to_mps(int absolute_speed);

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

                                scoutsim::SetPen::Response&);

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

            unsigned int rgb_to_grey(unsigned char r,

                                     unsigned char g,

                                     unsigned char b);

            bool isFront;

            int teleop_latch;

	        wxBitmap *path_bitmap;

	        bool ignore_behavior;

            //std::string current_teleop_scout;

            ros::NodeHandle node;

            wxImage emitter_image;

            wxBitmap emitter;

            Vector2 pos;

            float orient;

            // Each emitter has a unique id number, which is also displayed on its image.

            int emitter_id;

            float aperture;

            int distance;

            bool pen_on;

            bool emitter_visual_on;

            wxPen pen;

            ros::Publisher color_pub;

            ros::ServiceServer set_pen_srv;

            ros::WallTime last_command_time;

    };

    typedef boost::shared_ptr<Emitter> EmitterPtr;

}

#endif