RoboBuggy

  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)

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

  }