Example #1
0
File: Wave.cpp Project: cslux/ledcc
void Wave::createAnimation()
{
    float /*origin_x, origin_y,*/ distance, height, ripple_interval = 1.3;

    fillCubeArray(0x00);
//    QVector<QVector<QVector<quint8> > > tripleVector(iterations(),QVector<QVector<quint8> >(8, QVector<quint8>(8)));

    for (quint16 i=0; i<iterations(); i++)
    {
        if(m_abort)
            return;
        for (quint8 x=0; x<8; x++)
        {
            for (quint8 y=0; y<8; y++)
            {
                distance = distance2d(3.5,3.5,x,y)/9.899495*8;
                height = 4+sin(distance/ripple_interval+static_cast<float>(i)/50)*4;
                setBixel(x,y,static_cast<quint8>(height));
            }
        }
        waitMs(speed());
//        tripleVector[i] = cubeFrame;
        fillCubeArray(0x00);
    }
//    for (int i = 0; i < iterations(); i++) {
//        sendData(tripleVector[i]);
////        if(i/10 == 0)
////            waitMs(speed());
//    }
    Q_EMIT done();
}
Example #2
0
void sidewaves (int iterations, int delay)
{
	float origin_x, origin_y, distance, height, ripple_interval;
	int x,y,i;

	fill(0x00);

	for (i=0;i<iterations;i++)
	{

		origin_x = 3.5+sin((float)i/500)*4;
		origin_y = 3.5+cos((float)i/500)*4;
		
		for (x=0;x<8;x++)
		{
			for (y=0;y<8;y++)
			{
				distance = distance2d(origin_x,origin_y,x,y)/9.899495*8;
				ripple_interval =2;
				height = 4+sin(distance/ripple_interval+(float) i/50)*3.6;

				setvoxel(x,y,(int) height);
				setvoxel(x,y,(int) height);
					
			}
		}

		delay_ms(delay);
		fill(0x00);
	}
}
Example #3
0
// Display a sine wave running out from the center of the cube.
void ripples (int iterations, int delay)
{
	float origin_x, origin_y, distance, height, ripple_interval;
	int x,y,i;

	fill(0x00);

	for (i=0;i<iterations;i++)
	{
		for (x=0;x<8;x++)
		{
			for (y=0;y<8;y++)
			{
				distance = distance2d(3.5,3.5,x,y)/9.899495*8;
				//distance = distance2d(3.5,3.5,x,y);
				ripple_interval =1.3;
				height = 4+sin(distance/ripple_interval+(float) i/50)*4;

				setvoxel(x,y,(int) height);	
			}
		}
		delay_ms(delay);
		fill(0x00);
	}
}
void LocalPlanner::createPolarHistogram()
{  

	float bbx_rad = (max.x-min.x)*sqrt(2)/2; 
  float dist;
  
 
  polar_histogram.setZero();

  pcl::PointCloud<pcl::PointXYZ>::const_iterator it;
    
  for( it = final_cloud.begin(); it != final_cloud.end(); ++it)
  {   
    geometry_msgs::Point temp; 
    temp.x= it->x;
    temp.y = it->y;
    temp.z = it->z;
    dist = distance2d(pose,temp);
   
    if(dist < bbx_rad)
    { 
      int beta_z = floor((atan2(temp.x-pose.x,temp.y-pose.y)*180.0/PI)); //azimuthal angle
      int beta_e = floor((atan2(temp.z-pose.z,sqrt((temp.x-pose.x)*(temp.x-pose.x)+(temp.y-pose.y)*(temp.y-pose.y)))*180.0/PI));//elevation angle
    
      beta_z = beta_z + (alpha_res - beta_z%alpha_res);
      beta_e = beta_e + (alpha_res - beta_e%alpha_res); 
      
      int e = (180+beta_e)/alpha_res - 1 ;
      int z = (180+beta_z)/alpha_res - 1 ;

      polar_histogram.set(e,z,polar_histogram.get(e,z)+1);
    }
 }

}
Example #5
0
void build_buildings(region_t &region, rltk::random_number_generator &rng, const int n_buildings, const bool active, 
    std::vector<std::tuple<int,int,int>> &spawn_points, const std::size_t &civ_id, planet_t &planet) 
{
    std::cout << "Civ ID#: " << civ_id << "\n";
    int i=0;
    while (i<n_buildings) {
        auto hut = get_building_template(civ_id, planet, rng);
        xp::rex_sprite * building = &hut;
        bool ok = false;
        int tries = 0;
        int x,y,z;
        while (!ok) {
            ok = true;
            x = rng.roll_dice(1, REGION_WIDTH - 20) + 10;
            y = rng.roll_dice(1, REGION_HEIGHT - 20) + 10;
            z = get_ground_z(region, x, y);

            // Find lowest point
            for (int Y=0; Y < building->get_height(); ++Y) {
                for (int X = 0; X < building->get_width(); ++X) {
                    const int tmp_z = get_ground_z(region, x+X, y+Y);
                    if (z > tmp_z) z = tmp_z;
                }
            }

            // Check for solid or underwater
            for (int Y=0; Y < building->get_height(); ++Y) {
                for (int X = 0; X < building->get_width(); ++X) {
                    for (int Z = 0; Z < building->get_num_layers(); ++Z) {
                        const auto idx = mapidx(X+x, Y+y, Z+z);
                        if (region.solid[idx]) ok = false;
                        if (region.water_level[idx] > 0) ok = false;
                        if (region.tile_type[idx] == tile_type::SOLID) ok = false;
                    }
                }
            }

            // Not too close to the escape pod!
            if (distance2d(x,y, REGION_WIDTH/2, REGION_HEIGHT/2) < 15.0F) ok = false;

            if (!ok) ++tries;
            if (tries > 50) ok = true;
        }

        // Spawn the hut
        if (tries < 51) {
            std::cout << "Building passing civ_id: " << civ_id << "\n";
            const int n_spawn = build_building(*building, x, y, z, region, spawn_points, active, civ_id);
            i += (n_spawn -1);
        }
    }
}
Example #6
0
double MaximumSpread(double x[], double y[], int size){
int i;
int ans=0;
if(i<=1) return 0;
for(i=0;i<size;i++){
double distance=distance2d(x[i], y[i]);
if(distance>ans){
ans=distance;
}
}
return ans;

}
Example #7
0
void HUD_Lua_Class::add_entity_blip(short mtype, short intensity, short x, short y)
{
	world_point2d origin, target;
	origin.x = origin.y = 0;
	target.x = x;
	target.y = y;
	
	blip_info info;
	info.mtype = mtype;
	info.intensity = intensity;
	info.distance = distance2d(&origin, &target);
	info.direction = arctangent(x, y);
	
	m_blips.push_back(info);
}
double LocalPlanner::costFunction(int e, int z)
{ 
  double cost;
  int goal_z = floor(atan2(goal.x-pose.x,goal.y-pose.y)*180.0/PI); //azimuthal angle
  int goal_e = floor(atan2(goal.z-pose.z,sqrt((goal.x-pose.x)*(goal.x-pose.x)+(goal.y-pose.y)*(goal.y-pose.y)))*180.0/PI);//elevation angle

  waypt = getWaypointFromAngle(e,z);
  
  geometry_msgs::Point p;
  p.x = waypt.vector.x;
  p.y = waypt.vector.y;
  p.z = waypt.vector.z;

  if(velocity_x > 1.4 && braking_param )
  {
    //cost =   2*distance2d(p,goal)  +  8*(waypt.vector.z-pose.z); //working till 0.5
    double obs_dist = distance2d(p,obs);
    double energy_waypoint = 0.5*1.56*velocity_x*velocity_x + 1.56*9.81*waypt.vector.z;
    double energy_threshold = 0.5*1.56*3*3 + 1.56*9.81*goal.z;
    double energy_diff = energy_waypoint-energy_threshold;
    double ke_diff =  0.5*1.56*velocity_x*velocity_x - 0.5*1.56*1.4*1.4;
    double pe_diff =  1.56*9.81*pose.z - 1.56*9.81*goal.z;
    //cost =   -1*x_brake_cost_param*distance2d(p,obs)  +  z_brake_cost_param*(waypt.vector.z-pose.z);
   // if(energy_diff>=0)
   // cost =  1*x_brake_cost_param*(1/obs_dist)  +  -1*z_brake_cost_param*energy_diff;
   // if(energy_diff<=0)
   // cost =  1*x_brake_cost_param*(1/obs_dist)  +   z_brake_cost_param*energy_diff;
     
     cost = (1/obs_dist)  + x_brake_cost_param*ke_diff*(waypt.vector.x-pose.x) + z_brake_cost_param*pe_diff*(waypt.vector.z-pose.z);

  }
  else
  {
  
  if(velocity_x < 1.4)
  first_brake = false;
  
  if(e>-50)
  {
  cost = goal_cost_param*sqrt((goal_e-e)*(goal_e-e)+(goal_z-z)*(goal_z-z)) + smooth_cost_param*sqrt((p1.x-e)*(p1.x-e)+(p1.y-z)*(p1.y-z)) ; //Best working cost function
  }
  else
  cost = 10000;
  }

  return cost;  
}
void scenarioWaves::makeScenario(QList< QList<LedState> > * scenario, QList<int> *delay)
{
    int x = 0;
    int y = 0;
    int z = 0;


    int red = qrand()%255;
    int green = qrand()%255;
    int blue = qrand()%255;

    float distance, height, ripple_interval;
    int i;

    for (i=0;i<2000;i++)
    {
        QList<LedState> picture;
        for (x=0;x<m_xSize;x++)
        {
            for (z=0;z<m_ySize;z++)
            {
                distance = distance2d((float)m_ySize/2,(float)m_ySize/2,x,z)/9.899495*m_ySize;
                ripple_interval =1.3;
                height = (float)(m_zSize/2)+sin(distance/ripple_interval+(float) i/50)*(float)(m_zSize/2);

                LedState led = {x,(int)height, z,red,green,blue};
                picture.append(led);
            }
        }
        scenario->append(picture);
        delay->append(3);
        picture.clear();
    }

    emit doneCreating();
}
bool LocalPlanner::checkForCollision()
{
  bool avoid = false;
  geometry_msgs::Point temp;
  geometry_msgs::Point p;
  p.x = waypt.vector.x;
  p.y = waypt.vector.y;
  p.z = waypt.vector.z;

  pcl::PointCloud<pcl::PointXYZ>::iterator it;
  for( it = final_cloud.begin(); it != final_cloud.end(); ++it)
  {
    temp.x = it->x;
    temp.y = it->y;
    temp.z = it->z;
     
    if(distance2d(p,temp)< 0.5 && init != 0)
    { 
     avoid = true;
     break;
    }
  }  
  return avoid;
}
Example #11
0
/**
 * The goal is to return the best sample point that we can split this
 * edge on. Returns the index of said sample point.
 */
int Geom_2D::get_sample_point(Mesh::EdgeHandle &ehandle){
    /*
     * The goal is to convert these coordinates into 2 dimensions
     * and do the necessary calculations there. The position of the
     * translated points is arbitrary, we shall merely calculate a
     * distance then use a unit vector to find the relevant sample point
     */

    Mesh::HalfedgeHandle heh = mesh->halfedge_handle(ehandle, 0);

    //starting point. We position our edge along the x-axis.

    vector<Point_2D> p;
    p.push_back(Point_2D(0.0, 0.0, 0.0));  //0

    p.push_back(Point_2D(mesh->calc_edge_length(heh), 0.0, 0.0)); //1

    p.push_back(get_2D_point(heh, p[0], p[1])); //2

    Mesh::HalfedgeLoopIter heIt;
    heIt = mesh->hl_begin(heh);

    heIt ++;
    p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[2], p[1]));  //3

    heIt ++;
    p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[0], p[2])); //4

    Mesh::HalfedgeHandle heh2 = mesh->opposite_halfedge_handle(heh);

    p.push_back(get_2D_point(heh2, p[1], p[0])); //5
    heIt = mesh->hl_begin(heh2);

    heIt ++;
    p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[5], p[0])); //6

    heIt ++;
    p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[1], p[5])); //7

    /*
     * We aim for simplicity and accuracy over speed in this section.
     * cc and r contain the circumcenter and the radius of the circumcircles
     * of the provided triangles.
     */
    double cc[2*6], r[6];

    get_circumcircle(p[0], p[2], p[5], &cc[0], r[0]);
    get_circumcircle(p[1], p[2], p[5], &cc[2], r[1]);
    get_circumcircle(p[1], p[2], p[3], &cc[4], r[2]);
    get_circumcircle(p[1], p[7], p[5], &cc[6], r[3]);
    get_circumcircle(p[0], p[2], p[4], &cc[8], r[4]);
    get_circumcircle(p[0], p[6], p[5], &cc[10], r[5]);

    vector<Mesh::Point>* samps;
    samps = &(mesh->property(*samples, ehandle));
    double score;
    Point_2D p2d(0.0,0.0,0.0);
    int maxScore = 0;
    int maxIndex = 0;

    if (samps->size()==0){
        cout<<"*************************"<<endl<<"0 samples"<<endl;
        output_point(ehandle);
        cout<<"*************************"<<endl;
        //this should never happen....but it does.
        return -1;

    }
    for (int i = 0; i < samps->size(); i++){
        //first two circles if the sample points are in both we
        //score 5
        //void Geom_2D::mesh_to_plane(Mesh::HalfedgeHandle heh, Mesh::Point &samp, Point_2D &p0, Point_2D &p1, Point_2D &dest){
        score = 0;
        mesh_to_plane(heh, (*samps)[i], (p[0]), (p[1]), p2d);

        if (score_type == 1){
            if (distance2d(cc, p2d)<r[0]){
                score += 1.5;
            }
            if (distance2d(cc+2, p2d)<r[1]){
                score += 1.5;
            }
        }else if (score_type == 0){
            if (distance2d(cc, p2d)<r[0]){
                score ++;
            }
            if (distance2d(cc+2, p2d)<r[1]){
                score ++;
            }
        }else if (score_type == 2){
            if ((distance2d(cc, p2d)<r[0])&&(distance2d(cc+2, p2d)<r[1])){
                score += 5;
            }
        }

        for (int j = 2; j < 6; j++){
            if (distance2d(cc+(2*j), p2d)>r[j]){
                score++;
            }
        }
        if (score>maxScore){
            maxScore = score;
            maxIndex = i;
        }

    }
    //cout<<"max Score: "<<maxScore<<endl;
    //if ((maxIndex == 0)||(maxIndex == samps->size()-1)) {
    if(samps->size()==0){
        cout<<"*************************"<<endl;
        cout<<"******************index: "<<maxIndex<<endl;
        cout<<"*************************"<<endl;
        cout<<"samples size: "<<samps->size()<<endl;
        cout<<" is flippable "<< mesh->property(*is_flippable, ehandle)<<endl;
        cout<<" is Delaunay "<< mesh->property(*is_NDE, ehandle)<<endl;
        output_point(ehandle);
        cout<<"length: "<<mesh->calc_edge_length(ehandle)<<endl;
        cout<<"max score "<<maxScore<<endl;

    }
    return maxIndex;
}
void
motion_controller::running(void)
{
    cycle_start = ros::Time::now();

//  receive path and when received block path receiver until mission completed
    if (m_path && !block_path_receiver)
    {
        nav_msgs::Path temp_path = *m_path;
        std::vector<geometry_msgs::PoseStamped> temp_path_vector;
        extracted_path.clear();
        for( std::vector<geometry_msgs::PoseStamped>::iterator itr = temp_path.poses.begin() ; itr != temp_path.poses.end(); ++itr)
        {
            temp_path_vector.push_back(*itr);
        }

        if(!temp_path_vector.empty())
        {
             ROS_INFO("Path Received!");
        }
        p_count++;

        while(!temp_path_vector.empty())
        {
            extracted_path.push_back(temp_path_vector.back());
            temp_path_vector.pop_back();
        }
        block_path_receiver = true;
    }

//  switch status according to the command
    switch (m_move)
    {
        // move forward 
        case FORWARD:
        {
            // v is linear speed and gain is angular velocity
            v =  sqrt(dist)*v_max/(1 + gamma*k*k)*10000/PI;

            gain = 0.3265*v*k;
            if (fabs(gain) > 600)
            {
                gain = boost::math::sign(gain) * 600;
            }

            if(fabs(v)>2000)
            {
                v = boost::math::sign(v)*2000;
            }
            // set motor rotation velocity
            locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));

            // when the euler distance is less than 100mm, achieved waypoint
            if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
            {
                m_move = IDLE;
            }
            break;
        }

        case BACKWARD:
        {
            v =  sqrt(dist) * 0.75/(1+0.2*k_back*k_back) *10000/3.1415926;

            gain = 0.3265*v*k_back;

            if (fabs(gain) > 600)
            {
                gain = boost::math::sign(gain) * 600;
            }

            if(fabs(v)>2000)
            {
                v = boost::math::sign(v)*2000;
            }
            
            locomotor->set_vel(-floor(v)+floor(gain),floor(v)+floor(gain));
            if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
            {
                m_move = IDLE;
            }
            break;
        }

        case LIFTFORK:
        {
            // TODO: Lift height should be determined, fork stroke should be determined
            printf("Lift and fork!\n");

            locomotor->set_vel(0, 0);
            sleep(3);
//            locomotor->shut_down();
            lift_motor->power_on();
            if ((fork_count%2) == 1)
//          change -243720 to be -223720, wants to liftfork a little bit lower
                lift_motor->set_pos(-223720);
            else
                lift_motor->set_pos(-293720);

            sleep(10);

           printf("Start Test\n");

            // pose correction code inserted here  first make sure tag is attached vertically, second camera has no pitch angle relative to the vehicle
            

            if ((fork_count%2) == 1)
            {
                printf("Start Correction!\n");
                int count_detect = 0;
                while(ros::ok())
                {
                    if(abs_pose1)
                    {
                        Eigen::Quaternion<float> quat;
                        quat.w() = abs_pose1->pose.pose.orientation.w;
                        quat.x() = abs_pose1->pose.pose.orientation.x;
                        quat.y() = abs_pose1->pose.pose.orientation.y;
                        quat.z() = abs_pose1->pose.pose.orientation.z;

                        Eigen::Matrix3f Rotation;

                        Rotation = qToRotation(quat);

                        Eigen::Matrix3f FixTF;
                        FixTF << 1, 0,  0,
                                 0, 0, -1,
                                 0, 1,  0;

                        Rotation = FixTF * Rotation.inverse();

                        float yaw_error;
                    
                        yaw_error = getYawFromMat(Rotation);
                    
                        gain = -3265*(k_angle*yaw_error)/3.1415926;
                        if(fabs(gain)>150)
                        {
                            gain = boost::math::sign(gain) * 150;
                        }
                        locomotor->set_vel(floor(gain), floor(gain));
                        if (fabs(yaw_error*180/3.1415926) < 0.5)
                        {
                            locomotor->set_vel(0,0);
                            printf("Yaw %f corrected!\n", yaw_error*180/3.1415926);
                            break;
                        }
                    }
                    else
                    {
                        locomotor->set_vel(0, 0);
                        usleep(10000);
                        count_detect++;
                    }
                    ros::spinOnce();

                    if (count_detect>1000)
                    {
                        ROS_WARN("No Tag detected when stoped");
                        ros::shutdown();
                        exit(1);
                    }
                }
 
                count_detect = 0;
                while(ros::ok())
                {
                    if(abs_pose1)
                    {

                        Eigen::Quaternion<float> quat;
                        quat.w() = abs_pose1->pose.pose.orientation.w;
                        quat.x() = abs_pose1->pose.pose.orientation.x;
                        quat.y() = abs_pose1->pose.pose.orientation.y;
                        quat.z() = abs_pose1->pose.pose.orientation.z;

                        Eigen::Matrix3f Rotation;
                        Eigen::Vector3f translation;
                        translation << abs_pose1->pose.pose.position.x,
                                       abs_pose1->pose.pose.position.y,
                                       abs_pose1->pose.pose.position.z;

                        Rotation = qToRotation(quat);

                        translation = translation;

                        float x_error;
                    
                        x_error = translation[0];
                    
                        v = 0.25*(x_error-0.003)*10000/3.1415926;
//                        printf("x is %f\n", x_error);
//                        printf("v is %f\n\n", floor(v));
                        if(fabs(v)>150)
                        {
                            v = boost::math::sign(v) * 150;
                        }
                        locomotor->set_vel(floor(v), -floor(v));
                        if (fabs(x_error-0.003) < 0.003)
                        {
                            locomotor->set_vel(0, 0);
                            printf("x %f corrected!\n", (x_error-0.006));
                            break;
                        }
                    }
                    else
                    {
                        locomotor->set_vel(0, 0);
                        usleep(10000);
                        count_detect++;
                    }
                    ros::spinOnce();

                    if (count_detect>1000)
                    {
                        ROS_WARN("No Tag detected when stoped");
                        ros::shutdown();
                        exit(1);
                    }
                }
            }

            if ((fork_count%2) == 0)
            {
                int count_detect = 0;

                while(ros::ok())
                {
                    if (abs_pose)
                    {
                        Eigen::Quaternion<float> quat;
                        quat.w() = abs_pose->pose.pose.orientation.w;
                        quat.x() = abs_pose->pose.pose.orientation.x;
                        quat.y() = abs_pose->pose.pose.orientation.y;
                        quat.z() = abs_pose->pose.pose.orientation.z;

                        Eigen::Matrix3f Rotation;

                        Rotation = qToRotation(quat);

                        Eigen::Matrix3f fixTF;
                        fixTF << 1, 0,  0,
                                 0, -1, 0,
                                 0, 0, -1;

                        Rotation = Rotation.inverse()*fixTF;

                        m_state[2] = getYawFromMat(Rotation);
                        gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
                        if (fabs(gain) > 100)
                        {
                            gain = boost::math::sign(gain) * 100;
                        }
                        
                        if (fabs(gain) >100)
                        {
                            ros::shutdown();
                            exit(1);
                        }

                        locomotor->set_vel(floor(gain),floor(gain));
                        if (indicator(m_cmd[2], m_state[2], 0.008))
                        {
                            locomotor->set_vel(0, 0);
                            printf("Corrected!\n");
                            break;
                        }
                    }
                    else
                    {
                        locomotor->set_vel(0, 0);
                        usleep(10000);
                        count_detect++;
                    }
                    ros::spinOnce();

                    if (count_detect>1000)
                    {
                        locomotor->set_vel(0, 0);
                        ROS_WARN("No Tag detected when stoped");
                        //ros::shutdown();
                        //exit(1);
                    }
                }
            }

            // if we carry out hand hold barcode test, after correction, stop
//               ros::shutdown();
//               exit(1);

            locomotor->shut_down();
            sleep(0.5);

// comment following two lines can set make telefork not strech out
            telefork_motor->set_pos( boost::math::sign(lift_param)*(-386000) );
            sleep(15);

            if ((fork_count%2) == 1)
                 lift_motor->set_pos(-293720);
            else
                 lift_motor->set_pos(-223720);
            sleep(3);

            telefork_motor->set_pos(0);
            sleep(15);            

//            ros::shutdown();
//            exit(1);
            lift_motor->shut_down();
            locomotor->power_on();
            sleep(0.5);
            m_move = IDLE;

            break;
        }

        case TURN:
        {
            v = cos(alpha) * dist* k_dist *10000/PI;
            if(fabs(v)>300)
            {
                v = boost::math::sign(v)*300;
            }

            gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
            if (fabs(gain) > 400)
            {
                gain = boost::math::sign(gain) * 400;
            }
            locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));

            if (indicator(m_state[2],m_cmd[2],0.01))
            {
                m_move = IDLE;
            }
            break;
        }
  
        case IDLE:
        {
           locomotor->set_vel(0,0);
           if (extracted_path.size()!=0)
           {
               geometry_msgs::PoseStamped temp_pose = extracted_path.back();
               float yaw_ = 2*atan2(temp_pose.pose.orientation.z,temp_pose.pose.orientation.w);
               m_cmd << temp_pose.pose.position.x * m_resolution,
                        temp_pose.pose.position.y * m_resolution,
                        angle(yaw_,0);

               printf("Next Commanded Pose is (%f, %f, %f)\n", m_cmd[0], m_cmd[1], m_cmd[2]);
               if ( (fabs(m_cmd[0] - m_state[0])>0.5) && (fabs(m_cmd[1] - m_state[1])>0.5) )
               {
                   printf("Invalid commanded position received!\n");
                   locomotor->shut_down();
                   ros::shutdown();
                   exit(1);
               }
               if ( (fabs(m_cmd[0] - m_state[0])>0.5) || (fabs(m_cmd[1] - m_state[1])>0.5) )
               {
                   if (fabs(angle(m_cmd[2],m_state[2]))>0.5)
                   {
                       printf("Invalid commanded pose orientation received!\n");
                       locomotor->shut_down();
                       ros::shutdown();
                       exit(1);
                   }
                   if (fabs(m_cmd[0] - m_state[0])>0.5)
                   {
                       if (cos(m_state[2]) *  (m_cmd[0] - m_state[0]) > 0)
                           m_move = FORWARD;
                       else
                           m_move = BACKWARD;
                   }
                   else
                   {
                       if (sin(m_state[2]) *  (m_cmd[1] - m_state[1]) > 0)
                           m_move = FORWARD;
                       else
                           m_move = BACKWARD;
                   }
                   if (m_move == FORWARD)
                       printf("Move Forward!\n");
                   else
                       printf("Move Backward!\n");
               }
               else if (fabs(m_cmd[2] - m_state[2])>0.5)
               {
                   m_move = TURN;
                   printf("Turn Around!\n");
               }
               else if (temp_pose.pose.position.z!=0)
               {
                   m_move = LIFTFORK;
                   fork_count++;
                   lift_param = temp_pose.pose.position.z;
               }
               else
                    m_move = IDLE;
               extracted_path.pop_back();
           }
           else
           {
                if (p_count == 2)
                {
                    lift_motor->power_on();
                    lift_motor->set_pos(0);
                    sleep(12);
                    lift_motor->shut_down();
                    sleep(0.5);
                }

               block_path_receiver = false;
               geometry_msgs::PoseStamped pose_msg;
         
               pose_msg.header.frame_id = "world";
               pose_msg.header.stamp = ros::Time::now();
               pose_msg.pose.position.x = m_state[0];
               pose_msg.pose.position.y = m_state[1];
               if (block_path_receiver)
                  pose_msg.pose.position.z = 1;
               else
                  pose_msg.pose.position.z = 0;

               pose_msg.pose.orientation.w = cos(m_state[2]/2);
               pose_msg.pose.orientation.x = 0;
               pose_msg.pose.orientation.y = 0;
               pose_msg.pose.orientation.z = sin(m_state[2]/2);
               m_posePub.publish(pose_msg);
               printf("IDLE!\n");
               sleep(1);
               m_move = IDLE;
           }

           break;
        }
    }
        
    vel.first = (vel.first + locomotor->get_vel().first)/2;
    vel.second = (vel.second + locomotor->get_vel().second)/2;

    cycle_period = (ros::Time::now() - cycle_start).toSec();

    m_state[0] = m_state[0] + (-vel.second+vel.first)/2 * cos(m_state[2]) * 0.018849555 * cycle_period/60;
    m_state[1] = m_state[1] + (-vel.second+vel.first)/2 * sin(m_state[2]) * 0.018849555 * cycle_period/60;        
    m_state[2] = m_state[2] + (vel.first + vel.second)/0.653 * 0.018849555 * cycle_period/60;


    if (abs_pose)
    {
        int id;
        id = abs_pose->id - 3;

        Eigen::Quaternion<float> quat;
        quat.w() = abs_pose->pose.pose.orientation.w;
        quat.x() = abs_pose->pose.pose.orientation.x;
        quat.y() = abs_pose->pose.pose.orientation.y;
        quat.z() = abs_pose->pose.pose.orientation.z;

        Eigen::Matrix3f Rotation;
        Eigen::Vector3f translation;
        translation << abs_pose->pose.pose.position.x,
                       abs_pose->pose.pose.position.y,
                       abs_pose->pose.pose.position.z;

        Rotation = qToRotation(quat);

        translation = -Rotation.inverse()*translation;

        Eigen::Matrix3f fixTF;
        fixTF << 1, 0,  0,
                 0, -1, 0,
                 0, 0, -1;

        Rotation = Rotation.inverse()*fixTF;

        m_state[0] = translation[0] + m_resolution * (id%10);
        m_state[1] = translation[1] + m_resolution * floor(id/10.0);
        m_state[2] = getYawFromMat(Rotation) + 0.04;
    }

    geometry_msgs::PoseStamped pose_msg;
         
    pose_msg.header.frame_id = "world";
    pose_msg.header.stamp = ros::Time::now();
    pose_msg.pose.position.x = m_state[0];
    pose_msg.pose.position.y = m_state[1];
    if (block_path_receiver)
        pose_msg.pose.position.z = 1;
    else
        pose_msg.pose.position.z = 0;

    pose_msg.pose.orientation.w = cos(m_state[2]/2);
    pose_msg.pose.orientation.x = 0;
    pose_msg.pose.orientation.y = 0;
    pose_msg.pose.orientation.z = sin(m_state[2]/2);

    m_posePub.publish(pose_msg);

    delta_y = m_cmd[1]-m_state[1];
    if(fabs(m_cmd[1]-m_state[1]) > 1.6)
    {
        delta_y = boost::math::sign(m_cmd[1]-m_state[1]) * 1.6;
    }

    delta_x = m_cmd[0]-m_state[0];
    if(fabs(m_cmd[0]-m_state[0]) > 1.6)
    {
        delta_x = boost::math::sign(m_cmd[0]-m_state[0]) * 1.6;
    }

    beta = angle(m_cmd[2], atan2(delta_y, delta_x));

    alpha = angle(m_state[2], atan2(delta_y, delta_x));

    beta1 = angle(m_cmd[2]+PI, atan2(delta_y, delta_x));

    alpha1 = angle(m_state[2]+3.1415926, atan2(delta_y, delta_x));

    dist = sqrt(delta_x*delta_x + delta_y* delta_y);

    k = -1/dist * (k2*(alpha-atan(-k1*beta)) + sin(alpha)*(1 + k1/(1+(k1*beta) * (k1*beta))));
    k_back = -1/dist * (k2*(alpha1-atan2(-k1*beta1,1)) + sin(alpha1)*(1 + k1/(1+(k1*beta1) * (k1*beta1))));
}