Colony » Colony Scout

if(ROSBUILD)

  include(rosbuild.cmake)

  return()

endif()

cmake_minimum_required(VERSION 2.4.6)

include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

# Set the build type.  Options are:

#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage

#  Debug          : w/ debug symbols, w/o optimization

#  Release        : w/o debug symbols, w/ optimization

#  RelWithDebInfo : w/ debug symbols, w/ optimization

#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries

#set(ROS_BUILD_TYPE RelWithDebInfo)

rosbuild_init()

#set the default path for built executables to the "bin" directory

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

#set the default path for built libraries to the "lib" directory

set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

rosbuild_genmsg()

rosbuild_gensrv()

find_package(wxWidgets REQUIRED)

include(${wxWidgets_USE_FILE})

include_directories( ${wxWidgets_INCLUDE_DIRS} )

rosbuild_add_boost_directories()

rosbuild_add_executable(scoutsim_node src/scoutsim.cpp src/scout.cpp src/sim_frame.cpp)

rosbuild_link_boost(scoutsim_node thread)

target_link_libraries(scoutsim_node ${wxWidgets_LIBRARIES})

# @TODO are these useful or necessary? Determine; add as needed

#rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)

#rosbuild_add_executable(draw_square tutorials/draw_square.cpp)

#rosbuild_add_executable(mimic tutorials/mimic.cpp)

include $(shell rospack find mk)/cmake.mk

/*

 * 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 <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 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 nh_;

            wxImage scout_image_;

            wxBitmap scout_;

            Vector2 pos_;

            float orient_;

            float lin_vel_;

            float ang_vel_;

            bool pen_on_;

            wxPen pen_;

            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

/*

 * 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 sim_frame.h

 * @author Colony Project, CMU Robotics Club

 * @author Alex Zirbel

 */

#include <wx/wx.h>

#include <wx/event.h>

#include <wx/timer.h>

#include <ros/ros.h>

#include <std_srvs/Empty.h>

#include <scoutsim/Spawn.h>

#include <scoutsim/Kill.h>

#include <map>

#include "scout.h"

#define SCOUTSIM_NUM_SCOUTS 5

namespace scoutsim

{

    class SimFrame : public wxFrame

    {

        public:

            SimFrame(wxWindow* parent);

            ~SimFrame();

            std::string spawnScout(const std::string& name,

                    float x, float y, float angle);

        private:

            void onUpdate(wxTimerEvent& evt);

            void onPaint(wxPaintEvent& evt);

            void updateScouts();

            void clear();

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

            bool clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);

            bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);

            bool spawnCallback(scoutsim::Spawn::Request&, scoutsim::Spawn::Response&);

            bool killCallback(scoutsim::Kill::Request&, scoutsim::Kill::Response&);

            ros::NodeHandle nh_;

            wxTimer* update_timer_;

            wxBitmap path_bitmap_;

            wxImage path_image_;

            wxMemoryDC path_dc_;

            uint64_t frame_count_;

            ros::WallTime last_scout_update_;

            ros::ServiceServer clear_srv_;

            ros::ServiceServer reset_srv_;

            ros::ServiceServer spawn_srv_;

            ros::ServiceServer kill_srv_;

            typedef std::map<std::string, ScoutPtr> M_Scout;

            M_Scout scouts_;

            uint32_t id_counter_;

            wxImage scout_images_[SCOUTSIM_NUM_SCOUTS];

            float meter_;

            float width_in_meters_;

            float height_in_meters_;

    };

}

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.

Please see both licensing agreements below.

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

** BELOW IS THE LICENSING AGREEMENT FROM WILLOW GARAGE                      **

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

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.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

POSSIBILITY OF SUCH DAMAGE.

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

** BELOW IS THE LICENSING AGREEMENT FROM THE COLONY PROJECT                 **

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

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.

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.

/**

\mainpage

\htmlinclude manifest.html

\b scoutsim runs a simulator of the Scout robot for faster development.

<!-- 

In addition to providing an overview of your package,

this is the section where the specification and design/architecture 

should be detailed. While the original specification may be done on the

wiki, it should be transferred here once your package starts to take shape.

You can then link to this documentation page from the Wiki. 

-->

\section codeapi Code API

<!--

Provide links to specific auto-generated API documentation within your

package that is of particular interest to a reader. Doxygen will

document pretty much every part of your code, so do your best here to

point the reader to the actual API.

If your codebase is fairly large or has different sets of APIs, you

should use the doxygen 'group' tag to keep these APIs together. For

example, the roscpp documentation has 'libros' group.

-->

*/

<package>

  <description brief="scoutsim">

    scoutsim is a tool to simulate the Colony Scout platform, so that new

    behaviors can be prototyped rapidly.

  </description>

  <author>Alex Zirbel</author>

  <license>BSD</license>

  <review status="unreviewed" notes=""/>

  <url></url>

  <depend package="roscpp"/>

  <depend package="roslib"/>

  <depend package="rosconsole"/>

  <depend package="std_srvs"/>

  <rosdep name="wxwidgets"/>

  <export>

    <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp"/>

  </export>

  <platform os="ubuntu" version="9.04"/>

  <platform os="ubuntu" version="9.10"/>

  <platform os="ubuntu" version="10.04"/>

  <rosbuild2>

    <depend package="roscpp"/>

    <depend package="roslib"/>

    <depend package="rosconsole"/>

    <depend package="std_srvs"/>

    <msgs>

      msg/Color.msg  msg/Pose.msg  msg/Velocity.msg 

    </msgs>

    <srvs>

      srv/Kill.srv  

      srv/SetPen.srv  srv/Spawn.srv  srv/TeleportAbsolute.srv  srv/TeleportRelative.srv

    </srvs>

  </rosbuild2>

</package>

uint8 r

uint8 g

uint8 b

float32 x

float32 y

float32 theta

float32 linear_velocity

float32 angular_velocity

float32 linear

float32 angular

/*

 * 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 a single scout robot.

 * @file scout.cpp

 * @author Colony Project, CMU Robotics Club

 * @author Alex Zirbel

 */

#include "scoutsim/scout.h"

#include <wx/wx.h>

#define DEFAULT_PEN_R 0xb3

#define DEFAULT_PEN_G 0xb8

#define DEFAULT_PEN_B 0xff

namespace scoutsim

{

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

            const wxImage& scout_image,

            const Vector2& pos,

            float orient)

        : nh_(nh)

          , scout_image_(scout_image)

          , pos_(pos)

          , orient_(orient)

          , lin_vel_(0.0)

          , ang_vel_(0.0)

          , pen_on_(true)

          , pen_(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))

    {

        pen_.SetWidth(3);

        scout_ = wxBitmap(scout_image_);

        velocity_sub_ =

            nh_.subscribe("command_velocity",

                          1,

                          &Scout::velocityCallback, this);

        pose_pub_ = nh_.advertise<Pose>("pose", 1);

        color_pub_ = nh_.advertise<Color>("color_sensor", 1);

        set_pen_srv_ = nh_.advertiseService("set_pen",

                                            &Scout::setPenCallback,

                                            this);

        teleport_relative_srv_ =

            nh_.advertiseService("teleport_relative",

                                 &Scout::teleportRelativeCallback,

                                 this);

        teleport_absolute_srv_ =

            nh_.advertiseService("teleport_absolute",

                                 &Scout::teleportAbsoluteCallback,

                                 this);

        meter_ = scout_.GetHeight();

    }

    void Scout::velocityCallback(const VelocityConstPtr& vel)

    {

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

        lin_vel_ = vel->linear;

        ang_vel_ = vel->angular;

    }

    bool Scout::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;

    }

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

                                         scoutsim::TeleportRelative::Response&)

    {

        teleport_requests_.push_back(TeleportRequest(0,

                                                     0,

                                                     req.angular,

                                                     req.linear,

                                                     true));

        return true;

    }

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

                                         scoutsim::TeleportAbsolute::Response&)

    {

        teleport_requests_.push_back(TeleportRequest(req.x,

                                                     req.y,

                                                     req.theta,

                                                     0,

                                                     false));

        return true;

    }

    void Scout::update(double dt, wxMemoryDC& path_dc,

                        const wxImage& path_image, wxColour background_color,

                        float canvas_width, float canvas_height)

    {

        // first process any teleportation requests, in order

        V_TeleportRequest::iterator it = teleport_requests_.begin();

        V_TeleportRequest::iterator end = teleport_requests_.end();

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

        {

            const TeleportRequest& req = *it;

            Vector2 old_pos = pos_;

            if (req.relative)

            {

                orient_ += req.theta;

                pos_.x += sin(orient_ + PI/2.0) * req.linear;

                pos_.y += cos(orient_ + PI/2.0) * req.linear;

            }

            else

            {

                pos_.x = req.pos.x;

                pos_.y = std::max(0.0f, canvas_height - req.pos.y);

                orient_ = req.theta;

            }

            path_dc.SetPen(pen_);

            path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_,

                             old_pos.x * meter_, old_pos.y * meter_);

        }

        teleport_requests_.clear();

        if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0))

        {

            lin_vel_ = 0.0f;

            ang_vel_ = 0.0f;

        }

        Vector2 old_pos = pos_;

        orient_ = fmod(orient_ + ang_vel_ * dt, 2*PI);

        pos_.x += sin(orient_ + PI/2.0) * lin_vel_ * dt;

        pos_.y += cos(orient_ + PI/2.0) * lin_vel_ * dt;

        // Clamp to screen size

        if (pos_.x < 0 || pos_.x >= canvas_width

                || pos_.y < 0 || pos_.y >= canvas_height)

        {

            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x, pos_.y);

        }

        pos_.x = std::min(std::max(pos_.x, 0.0f), canvas_width);

        pos_.y = std::min(std::max(pos_.y, 0.0f), canvas_height);

        int canvas_x = pos_.x * meter_;

        int canvas_y = pos_.y * meter_;

        {

            wxImage rotated_image =

                scout_image_.Rotate(orient_ - PI/2.0,

                                    wxPoint(scout_image_.GetWidth() / 2,

                                            scout_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);

                    }

                }

            }

            scout_ = wxBitmap(rotated_image);

        }

        Pose p;

        p.x = pos_.x;

        p.y = canvas_height - pos_.y;

        p.theta = orient_;

        p.linear_velocity = lin_vel_;

        p.angular_velocity = ang_vel_;

        pose_pub_.publish(p);

        // Figure out (and publish) the color underneath the scout

        {

            wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight());

            Color color;

            color.r = path_image.GetRed(canvas_x, canvas_y);

            color.g = path_image.GetGreen(canvas_x, canvas_y);

            color.b = path_image.GetBlue(canvas_x, canvas_y);

            color_pub_.publish(color);

        }

        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",

                  nh_.getNamespace().c_str(), pos_.x, pos_.y, orient_);

        if (pen_on_)

        {

            if (pos_ != old_pos)

            {

                path_dc.SetPen(pen_);

                path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_,

                                 old_pos.x * meter_, old_pos.y * meter_);

            }

        }

    }

    void Scout::paint(wxDC& dc)

    {

        wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight());

        dc.DrawBitmap(scout_, pos_.x * meter_ - (scout_size.GetWidth() / 2),

                      pos_.y * meter_ - (scout_size.GetHeight() / 2), true);

    }

}

/*

 * 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 Runs the simulator, keeping track of all the scout robots.

 * @file scoutsim.cpp

 * @author Colony Project, CMU Robotics Club

 * @author Alex Zirbel

 */

#include <wx/app.h>

#include <wx/timer.h>

#include <ros/ros.h>

#include <boost/thread.hpp>

#include "scoutsim/sim_frame.h"

#ifdef __WXMAC__

#include <ApplicationServices/ApplicationServices.h>

#endif

class ScoutApp : public wxApp

{

public:

  char** local_argv_;

  ros::NodeHandlePtr nh_;

  ScoutApp()

  {

  }

  bool OnInit()

  {

#ifdef __WXMAC__

    ProcessSerialNumber PSN;

    GetCurrentProcess(&PSN);

    TransformProcessType(&PSN,kProcessTransformToForegroundApplication);

    SetFrontProcess(&PSN);

#endif

    // create our own copy of argv, with regular char*s.

    local_argv_ =  new char*[ argc ];

    for ( int i = 0; i < argc; ++i )

    {

      local_argv_[ i ] = strdup( wxString( argv[ i ] ).mb_str() );

    }

    ros::init(argc, local_argv_, "scoutsim");

    nh_.reset(new ros::NodeHandle);

    wxInitAllImageHandlers();

    scoutsim::SimFrame* frame = new scoutsim::SimFrame(NULL);

    SetTopWindow(frame);

    frame->Show();

    return true;

  }

  int OnExit()

  {

    for ( int i = 0; i < argc; ++i )

    {

      free( local_argv_[ i ] );

    }

    delete [] local_argv_;

    return 0;

  }

};

DECLARE_APP(ScoutApp);

IMPLEMENT_APP(ScoutApp);

/*

 * 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] = 

        {

            "box-turtle.png",

            "robot-turtle.png",

            "sea-turtle.png",

            "diamondback.png",

            "electric.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;

    }

}

string name

---

uint8 r

uint8 g

uint8 b

uint8 width

uint8 off

---

float32 x

float32 y

float32 theta

string name # Optional.  A unique name will be created and returned if this is empty

---

string name

float32 x

float32 y

float32 theta

---

float32 linear

float32 angular

---