Project

General

Profile

Revision 8ae3771a

ID8ae3771af6e41759df00f7b9892697aa48526696
Parent 797244ed
Child 46ed9b9b

Added by Yuyang Guo over 10 years ago

change type of bom reading to include sender

View differences:

scout/libscout/src/BomControl.cpp
45 45 
        node.serviceClient< ::messages::query_boms>(scoutname+"/query_BOM");
46 46 
}
47 47 

  		




  48
  
  
    
vector<uint32_t> BomControl::query()


  		




  
  48
  
    
BomReadings BomControl::query()


  		




  49
  49
  
    
{


  		




  50
  50
  
    
    ::messages::query_boms srv;


  		




  51
  51
  
    

  		




  ......




  60
  60
  
    
                 int(srv.response.readings.size()));


  		




  61
  61
  
    
    }


  		




  62
  62
  
    

  		




  63
  
  
    
    return srv.response.readings;


  		




  
  63
  
    
    BomReadings result;


  		




  
  64
  
    
    result.readings = srv.response.readings;


  		




  
  65
  
    
    result.senders = srv.response.senders;


  		




  
  66
  
    
    return result;


  		




  64
  67
  
    
}


  		




  65
  68
  
    

  		












  

    
      scout/libscout/src/BomControl.h
    
  





  37
  37
  
    
#include <ros/ros.h>


  		




  38
  38
  
    
#include <messages/query_boms.h>


  		




  39
  39
  
    

  		




  
  40
  
    
typedef struct BomReadings {


  		




  
  41
  
    
    std::vector<uint32_t> readings;


  		




  
  42
  
    
    std::vector<uint32_t> senders;


  		




  
  43
  
    
} BomReadings;


  		




  
  44
  
    

  		




  40
  45
  
    
class BomControl


  		




  41
  46
  
    
{


  		




  42
  47
  
    
    public:


  		




  ......




  45
  50
  
    
                          std::string scoutname);


  		




  46
  51
  
    

  		




  47
  52
  
    
        /** Use ROS to get the current readings. */


  		




  48
  
  
    
        std::vector<uint32_t> query();;


  		




  
  53
  
    
        BomReadings query();


  		




  49
  54
  
    

  		




  50
  55
  
    
        /**  */


  		




  51
  56
  
    
        //std::double getSourceEstimation();


  		












  

    
      scout/libscout/src/test_behaviors/ApproachEmitter.cpp
    
  





  39
  39
  
    
}


  		




  40
  40
  
    

  		




  41
  41
  
    
void ApproachEmitter::run() {


  		




  42
  
  
    
    vector<uint32_t> r;


  		




  
  42
  
    
    BomReadings bomR;


  		




  43
  43
  
    
    int r_weighted, l_weighted;


  		




  44
  44
  
    
    int speed = 20;


  		




  
  45
  
    
    vector<uint32_t> r;


  		




  
  46
  
    
    vector<uint32_t> s;


  		




  45
  47
  
    
    while (ok()) {


  		




  46
  
  
    
        r = bom->query();


  		




  
  48
  
    
        bomR = bom->query();


  		




  
  49
  
    
        r = bomR.readings;


  		




  
  50
  
    
        s = bomR.senders;


  		




  47
  51
  
    
        if (accumulate(r.begin(), r.end(), 0)) {


  		




  48
  
  
    
            l_weighted = r[0] *BOM_W1 + r[1]*BOM_W2 + r[2]*BOM_W3 + r[3]*BOM_W4 + r[4]*BOM_W5;


  		




  49
  
  
    
            r_weighted = r[9] *BOM_W1 + r[8]*BOM_W2 + r[7]*BOM_W3 + r[6]*BOM_W4 - r[5]*BOM_W5;


  		




  50
  
  
    
            motors->set_sides(min(r_weighted+speed, 100),


  		




  51
  
  
    
                              min(l_weighted+speed, 100),  MOTOR_PERCENT);


  		




  
  52
  
    
           l_weighted = r[0] *BOM_W1 + r[1]*BOM_W2 + r[2]*BOM_W3 + r[3]*BOM_W4 + r[4]*BOM_W5;


  		




  
  53
  
    
           r_weighted = r[9] *BOM_W1 + r[8]*BOM_W2 + r[7]*BOM_W3 + r[6]*BOM_W4 - r[5]*BOM_W5;


  		




  
  54
  
    
           motors->set_sides(min(r_weighted+speed, 100),


  		




  
  55
  
    
                             min(l_weighted+speed, 102),  MOTOR_PERCENT);


  		




  52
  56
  
    
        }


  		




  53
  57
  
    
    }


  		




  54
  58
  
    
}


  		












Also available in:  Unified diff