RoboBuggy

   class Turtle

   {

   public:

      Turtle(const ros::NodeHandle& nh,

             const QImage& turtle_image,

             const QPointF& pos,

             float orient);

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

                  qreal canvas_width, qreal canvas_height);

      void paint(QPainter &painter);

   private:

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

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

                          buggysim::SetPen::Response&);

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

                                    buggysim::TeleportRelative::Response&);

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

                                    buggysim::TeleportAbsolute::Response&);

      void rotateImage();

      ros::NodeHandle nh_;

      QImage turtle_image_;

      QImage turtle_rotated_image_;

      QPointF pos_;

      qreal orient_;

      qreal lin_vel_;

      //qreal ang_vel_;

      qreal turnAngle_;

      bool pen_on_;

      QPen pen_;

      ros::Subscriber angle_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, qreal _theta, qreal _linear, bool _relative)

            : pos(x, y)

            , theta(_theta)

            , linear(_linear)

            , relative(_relative)

         {}

         QPointF pos;

         qreal theta;

         qreal linear;

         bool relative;

      };

      typedef std::vector<TeleportRequest> V_TeleportRequest;

      V_TeleportRequest teleport_requests_;

   };

   typedef boost::shared_ptr<Turtle> TurtlePtr;

   class TurtleFrame : public QFrame

   {

      Q_OBJECT

   public:

      TurtleFrame(QWidget* parent = 0, Qt::WindowFlags f = 0);

      ~TurtleFrame();

      std::string spawnTurtle(const std::string& name, float x, float y, float angle);

   protected:

      void paintEvent(QPaintEvent* event);

   private slots:

      void onUpdate();

   private:

      void updateTurtles();

      void clear();

      bool hasTurtle(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(buggysim::Spawn::Request&, buggysim::Spawn::Response&);

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

      ros::NodeHandle nh_;

      QTimer* update_timer_;

      QImage path_image_;

      QPainter path_painter_;

      uint64_t frame_count_;

      ros::WallTime last_turtle_update_;

      ros::ServiceServer clear_srv_;

      ros::ServiceServer reset_srv_;

      ros::ServiceServer spawn_srv_;

      ros::ServiceServer kill_srv_;

      typedef std::map<std::string, TurtlePtr> M_Turtle;

      M_Turtle turtles_;

      uint32_t id_counter_;

      QVector<QImage> turtle_images_;

      float meter_;

      float width_in_meters_;

      float height_in_meters_;

   };

//  will be in AbstractBuggy.

// This file will focus on being a physics simulation engine.

   Turtle::Turtle(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);

      angle_sub_ = nh_.subscribe

                   ("command_turnAngle", 1, &Turtle::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", &Turtle::setPenCallback, this);

      teleport_relative_srv_ = nh_.advertiseService

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

      teleport_absolute_srv_ = nh_.advertiseService

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

      meter_ = turtle_image_.height();

      rotateImage();

   }

   void Turtle::turnAngleCallback(const buggymsgs::DirectionConstPtr& dir)

   {

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

      turnAngle_ = dir->turnAngle;

      //lin_vel_ = vel->linear;

      //ang_vel_ = vel->angular;

   }

   bool Turtle::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 Turtle::teleportRelativeCallback(buggysim::TeleportRelative::Request& req,

                                         buggysim::TeleportRelative::Response&)

   {

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

                                   true));

      return true;

   }

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

                                         buggysim::TeleportAbsolute::Response&)

   {

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

                                   false));

      return true;

   }

   void Turtle::rotateImage()

   {

      QTransform transform;

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

      turtle_rotated_image_ = turtle_image_.transformed(transform);

   }

   /**

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

    *       and include acceleration/joule dump

    */

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

                       qreal canvas_width, qreal canvas_height)

   {

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

   }

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

   }

   ros::NodeHandlePtr nh_;

   TurtleApp(int& argc, char** argv)

      : QApplication(argc, argv)

   {

      ros::init(argc, argv, "buggysim", ros::init_options::NoSigintHandler);

      nh_.reset(new ros::NodeHandle);

   }

   int exec()

   {

      buggysim::TurtleFrame frame;

      frame.show();

      return QApplication::exec();

   }

   TurtleApp app(argc, argv);

   return app.exec();

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

      : QFrame(parent, f)

      , path_image_(500, 500, QImage::Format_ARGB32)

      , path_painter_(&path_image_)

      , frame_count_(0)

      , id_counter_(0)

   {

      setFixedSize(500, 500);

      setWindowTitle("BuggySim Frame!");

      srand(time(NULL));

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

      QVector<QString> turtles;

      turtles.append("box-turtle.png");

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

      turtles.append("sea-turtle.png");

      turtles.append("diamondback.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);

      }

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

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

   }

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

         }

      }

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

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

      path_image_.fill(qRgb(r, g, b));

      update();

   }

   void TurtleFrame::onUpdate()

   {

      ros::spinOnce();

      updateTurtles();

      if (!ros::ok()) {

         close();

      }

   }

   void TurtleFrame::paintEvent(QPaintEvent* event)

   {

      QPainter painter(this);

      painter.drawImage(QPoint(0, 0), path_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;

      }

      bool modified = false;

      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(), path_painter_,

                                        path_image_, width_in_meters_, height_in_meters_);

      }

      if (modified) {

         update();

      }

      ++frame_count_;

   }

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

                                   std_srvs::Empty::Response&)

   {

      ROS_INFO("Clearing turtlesim.");

      clear();

      return true;

   }

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

      return true;

   }

enum State {

   FORWARD,

   STOP_FORWARD,

   TURN,

   STOP_TURN,

   g_pose = pose;

   return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1

          && fabsf(g_pose->theta - g_goal.theta) < 0.01;

   return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;

   ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);

   turtlesim::Velocity vel;

   vel.linear = linear;

   vel.angular = angular;

   vel_pub.publish(vel);

   if (hasStopped()) {

      ROS_INFO("Reached goal");

      g_state = TURN;

      g_goal.x = g_pose->x;

      g_goal.y = g_pose->y;

      g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);

      printGoal();

   } else {

      commandTurtle(vel_pub, 0, 0);

   }

   if (hasStopped()) {

      ROS_INFO("Reached goal");

      g_state = FORWARD;

      g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;

      g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;

      g_goal.theta = g_pose->theta;

      printGoal();

   } else {

      commandTurtle(vel_pub, 0, 0);

   }

   if (hasReachedGoal()) {

      g_state = STOP_FORWARD;

      commandTurtle(vel_pub, 0, 0);

   } else {

      commandTurtle(vel_pub, 1.0, 0.0);

   }

   if (hasReachedGoal()) {

      g_state = STOP_TURN;

      commandTurtle(vel_pub, 0, 0);

   } else {

      commandTurtle(vel_pub, 0.0, 0.4);

   }

   if (!g_pose) {

      return;

   }

   if (!g_first_goal_set) {

      g_first_goal_set = true;

      g_state = FORWARD;

      g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;

      g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;

      g_goal.theta = g_pose->theta;

      printGoal();

   }

   if (g_state == FORWARD) {

      forward(vel_pub);

   } else if (g_state == STOP_FORWARD) {

      stopForward(vel_pub);

   } else if (g_state == TURN) {

      turn(vel_pub);

   } else if (g_state == STOP_TURN) {

      stopTurn(vel_pub);

   }

   ros::init(argc, argv, "draw_square");

   ros::NodeHandle nh;

   ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback);

   ros::Publisher vel_pub =

      nh.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);

   ros::ServiceClient reset = nh.serviceClient<std_srvs::Empty>("reset");

   ros::Timer timer = nh.createTimer(ros::Duration(0.016),

                                     boost::bind(timerCallback, _1, vel_pub));

   std_srvs::Empty empty;

   reset.call(empty);

   ros::spin();

   Mimic();

   void poseCallback(const turtlesim::PoseConstPtr& pose);

   ros::Publisher vel_pub_;

   ros::Subscriber pose_sub_;

   ros::NodeHandle input_nh("input");

   ros::NodeHandle output_nh("output");

   vel_pub_ = output_nh.advertise<turtlesim::Velocity>("command_velocity", 1);

   pose_sub_ = input_nh.subscribe<turtlesim::Pose>("pose", 1, &Mimic::poseCallback,

               this);

   turtlesim::Velocity vel;

   vel.angular = pose->angular_velocity;

   vel.linear = pose->linear_velocity;

   vel_pub_.publish(vel);

   ros::init(argc, argv, "turtle_mimic");

   Mimic mimic;

   ros::spin();

 * Simple C++ Publisher;

 *   Publish keystrokes to the

#define KEYCODE_R 0x43

   TeleopTurtle();

   void keyLoop();

   ros::NodeHandle nh_;

   double linear_, angular_, l_scale_, a_scale_;

   ros::Publisher vel_pub_;

   linear_(0),

   angular_(0),

   l_scale_(2.0),

   a_scale_(2.0)

   nh_.param("scale_angular", a_scale_, a_scale_);

   nh_.param("scale_linear", l_scale_, l_scale_);

   vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);