RoboBuggy

 - Refactor into more AbstractClass/Class

 - Add maps/courses/walls

 - Give the buggy mass

 - Service to load with waypoints. 

 - Tight loop to determine the closest waypoint. navigate to it?

 - PID control of some sort. 

Planner

 - Given map, allow planning. 

 - Allow feedback

 - Optimizer (consider simulation feedback)

- Move Maps to a common folder

Using QTCreator

===============

Sometimes, it's just nice to have an IDE. 

Note that QTCreator must be launched from the shell, 

 so that it will inherit the correct environment. 

1. Install QTCreator

     sudo apt-get install qtcreator

2. Run it from the shell

     cd ~/buggyworkspace/src

     qtcreator

  src/adelaide.cpp

   class AbstractBuggy

      AbstractBuggy(const ros::NodeHandle& nh,

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

                  qreal canvas_width, qreal canvas_height);

      ros::WallTime last_command_time_;

      // TODO: Make teleporting in buggy.cpp, a protected function

   private:

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

                          buggysim::SetPen::Response&);

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

                                    buggysim::TeleportRelative::Response&);

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

                                    buggysim::TeleportAbsolute::Response&);

      QImage turtle_image_;

      QImage turtle_rotated_image_;

      //ros::Publisher color_pub_;

      ros::ServiceServer set_pen_srv_;

      ros::ServiceServer teleport_relative_srv_;

      ros::ServiceServer teleport_absolute_srv_;

  };

   // TODO: What should I even make this?

   typedef boost::shared_ptr<AbstractBuggy> TurtlePtr;

/*

 * 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

{

class Adelaide: public AbstractBuggy

{

public:

    ros::NodeHandle nh_;

    // Linear velocity, measured in meters/sec

    qreal lin_vel_;

    //qreal ang_vel_;

    qreal turnAngle_;

    ros::Subscriber angle_sub_;

    ros::Publisher pose_pub_;

    float meter_;

    //void turnAngleCallback(const buggymsgs::DirectionConstPtr& dir);

    Adelaide(const ros::NodeHandle& nh, const QImage& turtle_image,

             const QPointF& pos, float orient)

        : AbstractBuggy(nh, turtle_image, pos, orient)

        , lin_vel_(1.0)

        //, ang_vel_(0.0)

        , turnAngle_(0.0) // turn angle is stored in degrees

        //, pen_on_(true)

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

    {

        ROS_INFO("Namespace in childbuggy, unresolved: %s", nh_.getUnresolvedNamespace().data());

        ROS_INFO("Namespace in childbuggy: %s", nh_.getNamespace().data());

        //ROS_INFO("Namespace in childbuggy: %s", nh_.remapName());

        //ROS_INFO("Namespace in childbuggy, of arg: %s", nh.getNamespace().data());

        // TODO: fix this hack.

        angle_sub_ = nh_.subscribe

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

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

    }

    void turnAngleCallback(const buggymsgs::DirectionConstPtr& dir)

    {

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

        turnAngle_ = dir->turnAngle;

        //lin_vel_ = vel->linear;

        //ang_vel_ = vel->angular;

    }

    /**

    * TODO: convert this to take just an angle. And include drag.

    *       and include acceleration/joule dump

    */

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

                qreal canvas_width, qreal canvas_height)

    {

        qreal old_orient = orient_;

        bool modified = false;

        //qreal old_orient = orient_;

        // 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;

           QPointF old_pos = pos_;

           if (req.relative) {

              orient_ += req.theta;

              pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear;

              pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear;

           } else {

              pos_.setX(req.pos.x());

              pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y())));

              orient_ = req.theta;

           }

           path_painter.setPen(pen_);

           path_painter.drawLine(pos_ * meter_, old_pos * meter_);

           modified = true;

        }

        teleport_requests_.clear();

        */

        // Stop moving after 1 second since-last command.

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

        //{

        //  lin_vel_ = 0.0;

        //  ang_vel_ = 0.0;

        //}

        // Update my position

        QPointF old_pos = pos_;

        /* Convert coordinate systens

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

      pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt;

      pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt;

      */

        // TODO: what direction does orientation start at 0?

        // TODO: orient might have to become negative

        //orient_ = std::fmod(orient_ + ((turnAngle_/2*PI) * dt), 2*PI);

        orient_ = std::fmod(orient_ + ((-turnAngle_/(2*PI)) * dt), 2*PI);

        // TODO: why add pi/2? is this just because they got the

        //       sin/cos mixed up?

        //pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt;

        //pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt;

        pos_.rx() += std::cos(orient_) * lin_vel_ * dt;

        pos_.ry() -= std::sin(orient_) * lin_vel_ * dt;

        // Find linear velocity

        //qreal lin_vel;

        // Find angular velocity

        //qreal ang_vel;

        // Clamp to screen size

        // TODO: change to use another image, to detect walls.

        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_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0),

                           static_cast<double>(canvas_width)));

        pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0),

                           static_cast<double>(canvas_height)));

        // Publish pose of the turtle

        buggymsgs::Pose p;

        p.x = pos_.x();

        p.y = canvas_height - pos_.y();

        p.angle = orient_;

        p.linear_velocity = lin_vel_;

        //p.angular_velocity = ang_vel;

        pose_pub_.publish(p);

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

        /*{

        Color color;

        QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());

        color.r = qRed(pixel);

        color.g = qGreen(pixel);

        color.b = qBlue(pixel);

        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 (orient_ != old_orient) {

            rotateImage();

            modified = true;

        }

        if (pos_ != old_pos) {

            if (pen_on_) {

                path_painter.setPen(pen_);

                path_painter.drawLine(pos_ * meter_, old_pos * meter_);

            }

            modified = true;

        }

        return modified;

    }

};

}

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

                                const QImage& turtle_image,

                                const QPointF& pos,

                                float orient)

      //, lin_vel_(2.0)

      //, turnAngle_(0.0)

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

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

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

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

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

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

   }*/

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

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

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

   void AbstractBuggy::rotateImage()

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

       return false;

   void AbstractBuggy::paint(QPainter& painter)

#include "adelaide.cpp"

      // TODO: make it automatically pick up turtles?

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

      // TODO: associate buggy with specific model.

      //ROS_INFO("Name: %s", name.data());

      //ROS_INFO("Realname: %s", real_name.data());

      // Load the correct implementation of turtle

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

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

                             QPointF(x, y),

                             angle));