Colony » Colony Scout

/**

 * The code in this package was developed using the structure of Willow

 * Garage's turtlesim package.  It was modified by the CMU Robotics Club

 * to be used as a simulator for the Colony Scout robot.

 *

 * All redistribution of this code is limited to the terms of Willow Garage's

 * licensing terms, as well as under permission from the CMU Robotics Club.

 * 

 * Copyright (c) 2011 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * Copyright (c) 2009, Willow Garage, Inc.

 * All rights reserved.

 * 

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions are met:

 * 

 *    Redistributions of source code must retain the above copyright

 *       notice, this list of conditions and the following disclaimer.

 *    Redistributions in binary form must reproduce the above copyright

 *       notice, this list of conditions and the following disclaimer in the

 *       documentation and/or other materials provided with the distribution.

 *    Neither the name of the Willow Garage, Inc. nor the names of its

 *       contributors may be used to endorse or promote products derived from

 *       this software without specific prior written permission.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 */

#include "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"

        };

        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;

    }

}