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/buggy.h"

#include <QColor>

#include <QRgb>

#define DEFAULT_PEN_R 0xb3

#define DEFAULT_PEN_G 0xb8

#define DEFAULT_PEN_B 0xff

namespace buggysim

{

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

                                const QImage& turtle_image,

                                const QPointF& pos,

                                float orient)

      : nh_(nh)

      , turtle_image_(turtle_image)

      , pos_(pos)

      , orient_(orient)

      //, lin_vel_(2.0)

      //, ang_vel_(0.0)

      //, turnAngle_(0.0)

      , pen_on_(true)

      , pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))

   {

      pen_.setWidth(3);

      ROS_INFO("Namespace in abstract buggy: %s", nh.getNamespace().data());

      ROS_INFO("Namespace in abstract buggy, stored: %s", nh_.getNamespace().data());

      //angle_sub_ = nh_.subscribe

      //             ("command_turnAngle", 1, &AbstractBuggy::turnAngleCallback, this);

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

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

      set_pen_srv_ = nh_.advertiseService

                     ("set_pen", &AbstractBuggy::setPenCallback, this);

      teleport_relative_srv_ = nh_.advertiseService

                               ("teleport_relative", &AbstractBuggy::teleportRelativeCallback, this);

      teleport_absolute_srv_ = nh_.advertiseService

                               ("teleport_absolute", &AbstractBuggy::teleportAbsoluteCallback, this);

      meter_ = turtle_image_.height();

      rotateImage();

   }

   // This is a virtual method, to be implemented by the subclass

   /*void AbstractBuggy::turnAngleCallback(const buggymsgs::DirectionConstPtr& dir)

   {

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

      turnAngle_ = dir->turnAngle;

      //lin_vel_ = vel->linear;

      //ang_vel_ = vel->angular;

   }*/

   bool AbstractBuggy::setPenCallback(buggysim::SetPen::Request& req,

                               buggysim::SetPen::Response&)

   {

      pen_on_ = !req.off;

      if (req.off) {

         return true;

      }

      QPen pen(QColor(req.r, req.g, req.b));

      if (req.width != 0) {

         pen.setWidth(req.width);

      }

      pen_ = pen;

      return true;

   }

   bool AbstractBuggy::teleportRelativeCallback(buggysim::TeleportRelative::Request& req,

                                         buggysim::TeleportRelative::Response&)

   {

      teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear,

                                   true));

      return true;

   }

   bool AbstractBuggy::teleportAbsoluteCallback(buggysim::TeleportAbsolute::Request& req,

                                         buggysim::TeleportAbsolute::Response&)

   {

      teleport_requests_.push_back(TeleportRequest(req.x, req.y, req.theta, 0,

                                   false));

      return true;

   }

   void AbstractBuggy::rotateImage()

   {

      QTransform transform;

      transform.rotate(-orient_ * 180.0 / PI + 90.0);

      turtle_rotated_image_ = turtle_image_.transformed(transform);

   }

   bool AbstractBuggy::update(double dt, QPainter& path_painter, const QImage& path_image,

                       qreal canvas_width, qreal canvas_height)

   {

       return false;

   }

   void AbstractBuggy::paint(QPainter& painter)

   {

      QPointF p = pos_ * meter_;

      p.rx() -= 0.5 * turtle_rotated_image_.width();

      p.ry() -= 0.5 * turtle_rotated_image_.height();

      painter.drawImage(p, turtle_rotated_image_);

   }

}