RoboBuggy

/*

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

 */

#include "buggysim/sim_frame.h"

#include <QPointF>

#include <ros/package.h>

#include <cstdlib>

#include <ctime>

#define DEFAULT_BG_R 0x45

#define DEFAULT_BG_G 0x56

#define DEFAULT_BG_B 0xff

namespace buggysim

{

   TurtleFrame::TurtleFrame(QWidget* parent, Qt::WindowFlags f)

      : QFrame(parent, f)

      , frame_count_(0)

      , id_counter_(0)

   {

      // Same as image size

      setFixedSize(700, 500);

      setWindowTitle("BuggySim Frame!");

      srand(time(NULL));

      // QT window updater/timer

      update_timer_ = new QTimer(this);

      update_timer_->setInterval(16);

      update_timer_->start();

      connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate()));

      nh_.setParam("background_r", DEFAULT_BG_R);

      nh_.setParam("background_g", DEFAULT_BG_G);

      nh_.setParam("background_b", DEFAULT_BG_B);

      /* Set up map */

      // TODO: retrieve map names from param server

      std::string toBeRemovedMapName = "race";

      map_path_ = (ros::package::getPath("buggymaps") + "/maps/" +

                   toBeRemovedMapName + ".bmp").c_str();

      wall_path_ = (ros::package::getPath("buggymaps") + "/maps/" +

                toBeRemovedMapName + "_walls.bmp").c_str();

      wall_image_.load(wall_path_);

      map_image_ = new QImage(700, 500, QImage::Format_ARGB32);

      // TODO: paint waypoints

      // TODO: load turtles automatically/change to buggy images

      /* Load turtle images */

      // Note: buggies must be added here to be chosen.

      // TODO: associate buggy with specific model.

      QVector<QString> turtles;

      //turtles.append("robot-turtle.png");

      turtles.append("electric.png");

      //turtles.append("fuerte.png");

      //turtles.append("groovy.png");

      //turtles.append("hydro.svg");

      QString images_path = (ros::package::getPath("buggysim") + "/images/").c_str();

      for (size_t i = 0; i < turtles.size(); ++i) {

         QImage img;

         img.load(images_path + turtles[i]);

         turtle_images_.append(img);

      }

      // TODO: load meter/pixel ratio from parameter server

      meter_ = turtle_images_[0].height();

      clear();

      clear_srv_ = nh_.advertiseService("clear", &TurtleFrame::clearCallback, this);

      reset_srv_ = nh_.advertiseService("reset", &TurtleFrame::resetCallback, this);

      spawn_srv_ = nh_.advertiseService("spawn", &TurtleFrame::spawnCallback, this);

      kill_srv_ = nh_.advertiseService("kill", &TurtleFrame::killCallback, this);

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

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

      width_in_meters_ = (width() - 1) / meter_;

      height_in_meters_ = (height() - 1) / meter_;

      // Spawn turtle in a specific location.

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

      ROS_INFO("Past constructor!\n");

   }

   TurtleFrame::~TurtleFrame()

   {

      delete update_timer_;

   }

   bool TurtleFrame::spawnCallback(buggysim::Spawn::Request& req,

                                   buggysim::Spawn::Response& res)

   {

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

      if (name.empty()) {

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

         return false;

      }

      res.name = name;

      return true;

   }

   bool TurtleFrame::killCallback(buggysim::Kill::Request& req,

                                  buggysim::Kill::Response&)

   {

      M_Turtle::iterator it = turtles_.find(req.name);

      if (it == turtles_.end()) {

         ROS_ERROR("Tried to kill turtle [%s], which does not exist", req.name.c_str());

         return false;

      }

      turtles_.erase(it);

      update();

      return true;

   }

   bool TurtleFrame::hasTurtle(const std::string& name)

   {

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

   }

   std::string TurtleFrame::spawnTurtle(const std::string& name,

                                        float x,

                                        float y,

                                        float angle)

   {

      std::string real_name = name;

      if (real_name.empty()) {

         do {

            std::stringstream ss;

            // TODO: change how turtles are spawned

            ss << "buggy" << ++id_counter_;

            real_name = ss.str();

         } while (hasTurtle(real_name));

      } else {

         if (hasTurtle(real_name)) {

            return "";

         }

      }

      // Load the correct implementation of turtle to spawn into Screen

      TurtlePtr t(new Turtle(ros::NodeHandle(real_name),

                             // Added parameter for wall image

                             wall_image_,

                             turtle_images_[rand() % turtle_images_.size()],

                             QPointF(x, y),

                             angle));

      turtles_[real_name] = t;

      update();

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

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

      return real_name;

   }

   void TurtleFrame::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);

      // TODO: reload the map image

      //map_image_.fill(qRgb(r, g, b));

      map_image_->load(map_path_);

      update();

   }

   void TurtleFrame::onUpdate()

   {

      ros::spinOnce();

      updateTurtles();

      if (!ros::ok()) {

         close();

      }

   }

   void TurtleFrame::paintEvent(QPaintEvent* event)

   {

      QPainter painter(this);

      painter.drawImage(QPoint(0, 0), (*map_image_));

      M_Turtle::iterator it = turtles_.begin();

      M_Turtle::iterator end = turtles_.end();

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

         it->second->paint(painter);

      }

   }

   void TurtleFrame::updateTurtles()

   {

      if (last_turtle_update_.isZero()) {

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

         return;

      }

      //ROS_INFO("Inside update\n");

      bool modified = false;

      QPainter this_painter(map_image_);

      M_Turtle::iterator it = turtles_.begin();

      M_Turtle::iterator end = turtles_.end();

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

         modified |= it->second->update(0.001 * update_timer_->interval(), 

                    this_painter,

                    (*map_image_),

                    width_in_meters_, 

                    height_in_meters_);

      }

      if (modified) {

         update();

      }

      ++frame_count_;

      //ROS_INFO("Outside of update\n");

   }

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

                                   std_srvs::Empty::Response&)

   {

      ROS_INFO("Clearing turtlesim.");

      clear();

      return true;

   }

   // Don't use this code; it will explode.

   // This must be updated to handle more complicated sim

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

                                   std_srvs::Empty::Response&)

   {

      ROS_INFO("Resetting turtlesim.");

      turtles_.clear();

      id_counter_ = 0;

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

      clear();

      int a = 1/0;

      return true;

   }

}