Revision 93cebb99 scout/scoutsim/src/scout.cpp

View differences:

scout/scoutsim/src/scout.cpp
121 121
                                  &Scout::query_linesensor_callback,
122 122
                                  this);
123 123

  
124
        query_BOM_srv =
125
            node.advertiseService("query_BOM",
126
                                  &Scout::query_BOM_callback,
127
                                  this);
128

  
129

  
124 130
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
125 131
        {
126 132
            linesensor_readings.push_back(0);
127 133
        }
128 134

  
135
        for (unsigned int i = 0; i < NUM_BOMS; i++)
136
        {
137
            BOM_readings.push_back(0);
138
            BOM_senders.push_back(0);
139
        }
140

  
129 141
        // Initialize sonar
130 142
        sonar_position = 0;
131 143
        sonar_stop_l = 0;
......
276 288
        return true;
277 289
    }
278 290

  
291
    bool Scout::query_BOM_callback(::messages::query_boms::Request&,
292
                                   ::messages::query_boms::Response& res)
293
    {
294
        res.readings = BOM_readings;
295
        res.senders = BOM_senders;
296

  
297
        return true;
298
    }
299

  
279 300
    // Scale to linesensor value
280 301
    unsigned int Scout::rgb_to_grey(unsigned char r,
281 302
                                    unsigned char g,
......
429 450
        }
430 451
    }
431 452

  
432
    void Scout::update_BOM(int bom_index, bool bom_value) {
453
    void Scout::update_BOM(int bom_index,
454
                           unsigned int bom_value,
455
                           unsigned int sender) {
433 456
        ROS_INFO("BOM%d: %d", bom_index, bom_value);
457
        BOM_readings[bom_index] = bom_value;
458
        BOM_senders[bom_index] = sender;
434 459
    }
435 460

  
436

  
437 461
    /// Sends back the position of this scout so scoutsim can save
438 462
    /// the world state
439 463
    /// @todo remove dt param

Also available in: Unified diff