Colony » Colony Scout » Colony Scout OS

/*

 * This package was developed by the CMU Robotics Club project using code

 * from Willow Garage, Inc.  Please see licensing.txt for details.

 *

 * All rights reserved.

 *

 * @brief Keeps track of a single scout robot.

 * @file scout.cpp

 * @author Colony Project, CMU Robotics Club

 * @author Alex Zirbel

 */

#include "scout.h"

#include <wx/wx.h>

#define DEFAULT_PEN_R 0xb3

#define DEFAULT_PEN_G 0xb8

#define DEFAULT_PEN_B 0xff

namespace scoutsim

{

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

            const wxImage& scout_image,

            const Vector2& pos,

            float orient)

        : n_(nh)

          , scout_image_(scout_image)

          , pos_(pos)

          , orient_(orient)

          , lin_vel_(0.0)

          , ang_vel_(0.0)

          , pen_on_(true)

          , pen_(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))

    {

        pen_.SetWidth(3);

        scout_ = wxBitmap(scout_image_);

        motors_sub_ = n_.subscribe("set_motors", 1, &Scout::setMotors, this);

        /// @TODO take this out!

        velocity_sub_ =

            n_.subscribe("command_velocity",

                          1,

                          &Scout::velocityCallback, this);

        pose_pub_ = n_.advertise<Pose>("pose", 1);

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

        set_pen_srv_ = n_.advertiseService("set_pen",

                                            &Scout::setPenCallback,

                                            this);

        teleport_relative_srv_ =

            n_.advertiseService("teleport_relative",

                                 &Scout::teleportRelativeCallback,

                                 this);

        teleport_absolute_srv_ =

            n_.advertiseService("teleport_absolute",

                                 &Scout::teleportAbsoluteCallback,

                                 this);

        meter_ = scout_.GetHeight();

    }

    /**

     * A callback function that sets velocity based on a set_motors

     * request.

     */

    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)

    {

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

        if(msg->fl_set)

        {

            motor_fl_speed_ = msg->fl_speed;

        }

        if(msg->fr_set)

        {

            motor_fr_speed_ = msg->fr_speed;

        }

        if(msg->bl_set)

        {

            motor_bl_speed_ = msg->bl_speed;

        }

        if(msg->br_set)

        {

            motor_br_speed_ = msg->br_speed;

        }

        float l_speed = (float (motor_fl_speed_ + motor_bl_speed_)) / 2;

        float r_speed = (float (motor_fr_speed_ + motor_br_speed_)) / 2;

        /// @TODO Change this!

        lin_vel_ = (l_speed + r_speed) / 2;

        ang_vel_ = r_speed - l_speed;

    }

    void Scout::velocityCallback(const VelocityConstPtr& vel)

    {

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

        lin_vel_ = vel->linear;

        ang_vel_ = vel->angular;

    }

    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,

                               scoutsim::SetPen::Response&)

    {

        pen_on_ = !req.off;

        if (req.off)

        {

            return true;

        }

        wxPen pen(wxColour(req.r, req.g, req.b));

        if (req.width != 0)

        {

            pen.SetWidth(req.width);

        }

        pen_ = pen;

        return true;

    }

    bool Scout::teleportRelativeCallback(scoutsim::TeleportRelative::Request& req,

                                         scoutsim::TeleportRelative::Response&)

    {

        teleport_requests_.push_back(TeleportRequest(0,

                                                     0,

                                                     req.angular,

                                                     req.linear,

                                                     true));

        return true;

    }

    bool Scout::teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request& req,

                                         scoutsim::TeleportAbsolute::Response&)

    {

        teleport_requests_.push_back(TeleportRequest(req.x,

                                                     req.y,

                                                     req.theta,

                                                     0,

                                                     false));

        return true;

    }

    void Scout::update(double dt, wxMemoryDC& path_dc,

                        const wxImage& path_image, wxColour background_color,

                        float canvas_width, float canvas_height)

    {

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

            Vector2 old_pos = pos_;

            if (req.relative)

            {

                orient_ += req.theta;

                pos_.x += sin(orient_ + PI/2.0) * req.linear;

                pos_.y += cos(orient_ + PI/2.0) * req.linear;

            }

            else

            {

                pos_.x = req.pos.x;

                pos_.y = std::max(0.0f, canvas_height - req.pos.y);

                orient_ = req.theta;

            }

            path_dc.SetPen(pen_);

            path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_,

                             old_pos.x * meter_, old_pos.y * meter_);

        }

        teleport_requests_.clear();

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

        {

            lin_vel_ = 0.0f;

            ang_vel_ = 0.0f;

        }

        Vector2 old_pos = pos_;

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

        pos_.x += sin(orient_ + PI/2.0) * lin_vel_ * dt;

        pos_.y += cos(orient_ + PI/2.0) * lin_vel_ * dt;

        // Clamp to screen size

        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_.x = std::min(std::max(pos_.x, 0.0f), canvas_width);

        pos_.y = std::min(std::max(pos_.y, 0.0f), canvas_height);

        int canvas_x = pos_.x * meter_;

        int canvas_y = pos_.y * meter_;

        {

            wxImage rotated_image =

                scout_image_.Rotate(orient_ - PI/2.0,

                                    wxPoint(scout_image_.GetWidth() / 2,

                                            scout_image_.GetHeight() / 2),

                                    false);

            for (int y = 0; y < rotated_image.GetHeight(); ++y)

            {

                for (int x = 0; x < rotated_image.GetWidth(); ++x)

                {

                    if (rotated_image.GetRed(x, y) == 255

                            && rotated_image.GetBlue(x, y) == 255

                            && rotated_image.GetGreen(x, y) == 255)

                    {

                        rotated_image.SetAlpha(x, y, 0);

                    }

                }

            }

            scout_ = wxBitmap(rotated_image);

        }

        Pose p;

        p.x = pos_.x;

        p.y = canvas_height - pos_.y;

        p.theta = orient_;

        p.linear_velocity = lin_vel_;

        p.angular_velocity = ang_vel_;

        pose_pub_.publish(p);

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

        {

            wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight());

            Color color;

            color.r = path_image.GetRed(canvas_x, canvas_y);

            color.g = path_image.GetGreen(canvas_x, canvas_y);

            color.b = path_image.GetBlue(canvas_x, canvas_y);

            color_pub_.publish(color);

        }

        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",

                  n_.getNamespace().c_str(), pos_.x, pos_.y, orient_);

        if (pen_on_)

        {

            if (pos_ != old_pos)

            {

                path_dc.SetPen(pen_);

                path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_,

                                 old_pos.x * meter_, old_pos.y * meter_);

            }

        }

    }

    void Scout::paint(wxDC& dc)

    {

        wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight());

        dc.DrawBitmap(scout_, pos_.x * meter_ - (scout_size.GetWidth() / 2),

                      pos_.y * meter_ - (scout_size.GetHeight() / 2), true);

    }

}