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.

 *

 * @brief Keeps track of the frame (visualization of the whole environment).

 * @file sim_frame.cpp

 * @author Colony Project, CMU Robotics Club

 * @author Alex Zirbel

 */

#include "scoutsim/sim_frame.h"

#include <ros/package.h>

#include <cstdlib>

#include <ctime>

#define DEFAULT_BG_R 0x45

#define DEFAULT_BG_G 0x56

#define DEFAULT_BG_B 0xff

namespace scoutsim

{

    SimFrame::SimFrame(wxWindow* parent)

        : wxFrame(parent, wxID_ANY, wxT("ScoutSim"), wxDefaultPosition,

                  wxSize(500, 500), wxDEFAULT_FRAME_STYLE & ~wxRESIZE_BORDER)

          , frame_count_(0)

          , id_counter_(0)

    {

        srand(time(NULL));

        update_timer_ = new wxTimer(this);

        update_timer_->Start(16);

        Connect(update_timer_->GetId(), wxEVT_TIMER,

                wxTimerEventHandler(SimFrame::onUpdate), NULL, this);

        Connect(wxEVT_PAINT, wxPaintEventHandler(SimFrame::onPaint),

                NULL, this);

        nh_.setParam("background_r", DEFAULT_BG_R);

        nh_.setParam("background_g", DEFAULT_BG_G);

        nh_.setParam("background_b", DEFAULT_BG_B);

        std::string scouts[SCOUTSIM_NUM_SCOUTS] = 

        {

            "scout.png"

            "scout_forreal.png"

        };

        std::string images_path = ros::package::getPath("scoutsim")+"/images/";

        for (size_t i = 0; i < SCOUTSIM_NUM_SCOUTS; ++i)

        {

            scout_images_[i].LoadFile(

                wxString::FromAscii((images_path + scouts[i]).c_str()));

            scout_images_[i].SetMask(true);

            scout_images_[i].SetMaskColour(255, 255, 255);

        }

        meter_ = scout_images_[0].GetHeight();

        path_bitmap_ = wxBitmap(GetSize().GetWidth(), GetSize().GetHeight());

        path_dc_.SelectObject(path_bitmap_);

        clear();

        clear_srv_ = nh_.advertiseService("clear",

                                          &SimFrame::clearCallback, this);

        reset_srv_ = nh_.advertiseService("reset",

                                          &SimFrame::resetCallback, this);

        spawn_srv_ = nh_.advertiseService("spawn",

                                          &SimFrame::spawnCallback, this);

        kill_srv_ = nh_.advertiseService("kill",

                                         &SimFrame::killCallback, this);

        ROS_INFO("Starting scoutsim with node name %s",

                 ros::this_node::getName().c_str()) ;

        width_in_meters_ = GetSize().GetWidth() / meter_;

        height_in_meters_ = GetSize().GetHeight() / meter_;

        spawnScout("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);

    }

    SimFrame::~SimFrame()

    {

        delete update_timer_;

    }

    bool SimFrame::spawnCallback(scoutsim::Spawn::Request& req,

                                 scoutsim::Spawn::Response& res)

    {

        std::string name = spawnScout(req.name, req.x, req.y, req.theta);

        if (name.empty())

        {

            ROS_ERROR("A scout named [%s] already exists", req.name.c_str());

            return false;

        }

        res.name = name;

        return true;

    }

    bool SimFrame::killCallback(scoutsim::Kill::Request& req,

                                scoutsim::Kill::Response&)

    {

        M_Scout::iterator it = scouts_.find(req.name);

        if (it == scouts_.end())

        {

            ROS_ERROR("Tried to kill scout [%s], which does not exist",

                      req.name.c_str());

            return false;

        }

        scouts_.erase(it);

        return true;

    }

    bool SimFrame::hasScout(const std::string& name)

    {

        return scouts_.find(name) != scouts_.end();

    }

    std::string SimFrame::spawnScout(const std::string& name, float x,

                                       float y, float angle)

    {

        std::string real_name = name;

        if (real_name.empty())

        {

            do

            {

                std::stringstream ss;

                ss << "scout" << ++id_counter_;

                real_name = ss.str();

            } while (hasScout(real_name));

        }

        else

        {

            if (hasScout(real_name))

            {

                return "";

            }

        }

        ScoutPtr t(new Scout(ros::NodeHandle(real_name),

                   scout_images_[rand() % SCOUTSIM_NUM_SCOUTS],

                   Vector2(x, y), angle));

        scouts_[real_name] = t;

        ROS_INFO("Spawning scout [%s] at x=[%f], y=[%f], theta=[%f]",

                 real_name.c_str(), x, y, angle);

        return real_name;

    }

    void SimFrame::clear()

    {

        int r = DEFAULT_BG_R;

        int g = DEFAULT_BG_G;

        int b = DEFAULT_BG_B;

        nh_.param("background_r", r, r);

        nh_.param("background_g", g, g);

        nh_.param("background_b", b, b);

        path_dc_.SetBackground(wxBrush(wxColour(r, g, b)));

        path_dc_.Clear();

    }

    void SimFrame::onUpdate(wxTimerEvent& evt)

    {

        ros::spinOnce();

        updateScouts();

        if (!ros::ok())

        {

            Close();

        }

    }

    void SimFrame::onPaint(wxPaintEvent& evt)

    {

        wxPaintDC dc(this);

        dc.DrawBitmap(path_bitmap_, 0, 0, true);

        M_Scout::iterator it = scouts_.begin();

        M_Scout::iterator end = scouts_.end();

        for (; it != end; ++it)

        {

            it->second->paint(dc);

        }

    }

    void SimFrame::updateScouts()

    {

        if (last_scout_update_.isZero())

        {

            last_scout_update_ = ros::WallTime::now();

            return;

        }

        if (frame_count_ % 3 == 0)

        {

            path_image_ = path_bitmap_.ConvertToImage();

            Refresh();

        }

        M_Scout::iterator it = scouts_.begin();

        M_Scout::iterator end = scouts_.end();

        for (; it != end; ++it)

        {

            it->second->update(0.016, path_dc_, path_image_,

                               path_dc_.GetBackground().GetColour(),

                               width_in_meters_, height_in_meters_);

        }

        ++frame_count_;

    }

    bool SimFrame::clearCallback(std_srvs::Empty::Request&,

                                 std_srvs::Empty::Response&)

    {

        ROS_INFO("Clearing scoutsim.");

        clear();

        return true;

    }

    bool SimFrame::resetCallback(std_srvs::Empty::Request&,

                                 std_srvs::Empty::Response&)

    {

        ROS_INFO("Resetting scoutsim.");

        scouts_.clear();

        id_counter_ = 0;

        spawnScout("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);

        clear();

        return true;

    }

}