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)

        : node (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 = node.subscribe("set_motors", 1, &Scout::setMotors, this);

        pose_pub = node.advertise<Pose>("pose", 1);

        color_pub = node.advertise<Color>("color_sensor", 1);

        set_pen_srv = node.advertiseService("set_pen",

                                            &Scout::setPenCallback,

                                            this);

        teleport_relative_srv =

            node.advertiseService("teleport_relative",

                                 &Scout::teleportRelativeCallback,

                                 this);

        teleport_absolute_srv =

            node.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;

        }

        // Assume that the two motors on the same side will be set to

        // roughly the same speed. Does not account for slip conditions

        // when they are set to different speeds.

        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;

        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;

        // Set the linear and angular speeds

        lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;

        ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);

    }

    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;

    }

    /// @TODO remove

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

                                         scoutsim::TeleportRelative::Response&)

    {

        teleport_requests.push_back(TeleportRequest(0,

                                                     0,

                                                     req.angular,

                                                     req.linear,

                                                     true));

        return true;

    }

    /// @TODO remove

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

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

    }

}