Colony » Colony Scout » Colony Scout OS

        node.advertise<motors::set_motors>("set_motors", QUEUE_SIZE);

        motors.set_sides(20, 80);

include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)

include_directories(${MOTORS_INCLUDE_DIRS})

link_directories(${MOTORS_LIBRARY_DIRS})

target_link_libraries(scoutsim_node ${MOTORS_LIBRARIES})

target_link_libraries(scoutsim_node motors)

float32 angular

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

        teleport_relative_srv_ =

            n_.advertiseService("teleport_relative",

        teleport_absolute_srv_ =

            n_.advertiseService("teleport_absolute",

        meter_ = scout_.GetHeight();

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

            motor_fl_speed_ = msg->fl_speed;

            motor_fr_speed_ = msg->fr_speed;

            motor_bl_speed_ = msg->bl_speed;

            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;

        pen_on_ = !req.off;

        pen_ = pen;

        teleport_requests_.push_back(TeleportRequest(0,

        teleport_requests_.push_back(TeleportRequest(req.x,

        V_TeleportRequest::iterator it = teleport_requests_.begin();

        V_TeleportRequest::iterator end = teleport_requests_.end();

            Vector2 old_pos = pos_;

                orient_ += req.theta;

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

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

                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;

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

                scout_image_.Rotate(orient_ - PI/2.0,

                                    wxPoint(scout_image_.GetWidth() / 2,

                                            scout_image_.GetHeight() / 2),

            scout_ = wxBitmap(rotated_image);

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

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

            color_pub_.publish(color);

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

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

#ifndef SCOUTSIM_SCOUT_H

#define SCOUTSIM_SCOUT_H

#include <scoutsim/Velocity.h>

        Vector2(float _x, float _y)

            : x(_x)

              , y(_y)

            void velocityCallback(const VelocityConstPtr& vel);

            ros::NodeHandle n_;

            wxImage scout_image_;

            wxBitmap scout_;

            Vector2 pos_;

            float orient_;

            int motor_fl_speed_;

            int motor_fr_speed_;

            int motor_bl_speed_;

            int motor_br_speed_;

            float lin_vel_;

            float ang_vel_;

            bool pen_on_;

            wxPen pen_;

            ros::Subscriber motors_sub_;

            ros::Subscriber velocity_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_;

                TeleportRequest(float x, float y, float _theta,

                                float _linear, bool _relative)

                      , theta(_theta)

                      , linear(_linear)

                      , relative(_relative)

            V_TeleportRequest teleport_requests_;

  char** local_argv_;

  ros::NodeHandlePtr nh_;

    local_argv_ =  new char*[ argc ];

      local_argv_[ i ] = strdup( wxString( argv[ i ] ).mb_str() );

    ros::init(argc, local_argv_, "scoutsim");

    nh_.reset(new ros::NodeHandle);

      free( local_argv_[ i ] );

    delete [] local_argv_;

          , frame_count_(0)

          , id_counter_(0)

        update_timer_ = new wxTimer(this);

        update_timer_->Start(16);

        Connect(update_timer_->GetId(), wxEVT_TIMER,

        nh_.setParam("background_r", DEFAULT_BG_R);

        nh_.setParam("background_g", DEFAULT_BG_G);

        nh_.setParam("background_b", DEFAULT_BG_B);

            scout_images_[i].LoadFile(

            scout_images_[i].SetMask(true);

            scout_images_[i].SetMaskColour(255, 255, 255);

        meter_ = scout_images_[0].GetHeight();

        path_bitmap_ = wxBitmap(GetSize().GetWidth(), GetSize().GetHeight());

        path_dc_.SelectObject(path_bitmap_);

        clear_srv_ = nh_.advertiseService("clear",

        reset_srv_ = nh_.advertiseService("reset",

        spawn_srv_ = nh_.advertiseService("spawn",

        kill_srv_ = nh_.advertiseService("kill",

        width_in_meters_ = GetSize().GetWidth() / meter_;

        height_in_meters_ = GetSize().GetHeight() / meter_;

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

        delete update_timer_;

        M_Scout::iterator it = scouts_.find(req.name);

        if (it == scouts_.end())

        scouts_.erase(it);

        return scouts_.find(name) != scouts_.end();

                ss << "scout" << ++id_counter_;

                   scout_images_[rand() % SCOUTSIM_NUM_SCOUTS],

        scouts_[real_name] = t;

        nh_.param("background_r", r, r);

        nh_.param("background_g", g, g);

        nh_.param("background_b", b, b);

        path_dc_.SetBackground(wxBrush(wxColour(r, g, b)));

        path_dc_.Clear();

        dc.DrawBitmap(path_bitmap_, 0, 0, true);

        M_Scout::iterator it = scouts_.begin();

        M_Scout::iterator end = scouts_.end();

        if (last_scout_update_.isZero())

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

        if (frame_count_ % 3 == 0)

            path_image_ = path_bitmap_.ConvertToImage();

        M_Scout::iterator it = scouts_.begin();

        M_Scout::iterator end = scouts_.end();

            it->second->update(0.016, path_dc_, path_image_,

                               path_dc_.GetBackground().GetColour(),

                               width_in_meters_, height_in_meters_);

        ++frame_count_;

        scouts_.clear();

        id_counter_ = 0;

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

            ros::NodeHandle nh_;

            wxTimer* update_timer_;

            wxBitmap path_bitmap_;

            wxImage path_image_;

            wxMemoryDC path_dc_;

            uint64_t frame_count_;

            ros::WallTime last_scout_update_;

            ros::ServiceServer clear_srv_;

            ros::ServiceServer reset_srv_;

            ros::ServiceServer spawn_srv_;

            ros::ServiceServer kill_srv_;

            M_Scout scouts_;

            uint32_t id_counter_;

            wxImage scout_images_[SCOUTSIM_NUM_SCOUTS];

            float meter_;

            float width_in_meters_;

            float height_in_meters_;