testscdspso.cpp

Go to the documentation of this file.

#include "testscdspso/testscdspso.h"

#define PI 3.1415926

// Register the application
PLUGINLIB_EXPORT_CLASS(testscdspso::TestSCDSPSO, micros_swarm::Application)

namespace testscdspso{

    TestSCDSPSO::TestSCDSPSO() {}

    TestSCDSPSO::~TestSCDSPSO() {}

    void TestSCDSPSO::init() {}

    void TestSCDSPSO::stop() {}
    
    void TestSCDSPSO::loop(const ros::TimerEvent&)
    {
        static int i=0;
        if(i<50)
            i++;
        micros_swarm::SCDSPSODataTuple data;
        data.val = get_id() + i;
        tuple.put("test_scds_pso", data);

        micros_swarm::SCDSPSODataTuple best = tuple.get("test_scds_pso");
        std::cout<<"robot: "<<get_id()<<", best: "<<best.val<<std::endl;
    }

    void TestSCDSPSO::baseCallback(const nav_msgs::Odometry& lmsg)
    {
        float x=lmsg.pose.pose.position.x;
        float y=lmsg.pose.pose.position.y;

        float vx=lmsg.twist.twist.linear.x;
        float vy=lmsg.twist.twist.linear.y;

        micros_swarm::Base l(x, y, 0, vx, vy, 0, 1);
        set_base(l);
    }

    float TestSCDSPSO::fitness(const std::vector<float>& vec)
    {
        float x = vec[0];
        float y = vec[1];
        float fitness = -(20+x*x+y*y-10*cos(2*PI*x)-10*cos(2*PI*y));
        return fitness;
    }
    
    void TestSCDSPSO::start()
    {
        agent.set_param(1, 1.49445, 1.49445);
        agent.set_dim(2);
        agent.set_fitness(boost::bind(&TestSCDSPSO::fitness, this, _1));

        std::vector<float> max_pos;
        max_pos.resize(2);
        max_pos[0] = 5.12;
        max_pos[1] = 5.12;
        agent.set_max_pos(max_pos);

        std::vector<float> min_pos;
        min_pos.resize(2);
        min_pos[0] = -5.12;
        min_pos[1] = -5.12;
        agent.set_min_pos(min_pos);

        std::vector<float> max_vel;
        max_vel.resize(2);
        max_vel[0] = 1;
        max_vel[1] = 1;
        agent.set_max_vel(max_vel);

        std::vector<float> min_vel;
        min_vel.resize(2);
        min_vel[0] = -1;
        min_vel[1] = -1;
        agent.set_min_vel(min_vel);

        agent.rand_init();
        agent.start(1500);
    }
};