Fawkes API Fawkes Development Version
laser_mapper.cpp
1
2/***************************************************************************
3 * laser_mapper.cpp - Mapper LaserProxy to LaserInterface
4 *
5 * Created: Tue Oct 21 00:50:26 2008
6 * Copyright 2006-2008 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "laser_mapper.h"
24
25#include <core/exceptions/software.h>
26#include <interfaces/Laser360Interface.h>
27#include <libplayerc++/playerc++.h>
28#include <utils/math/angle.h>
29
30/** @class PlayerLaserMapper "laser_mapper.h"
31 * Laser mapper for player integration.
32 * This class is used to map a Player lsaer proxy to a Fawkes
33 * Laser360Interface.
34 * @author Tim Niemueller
35 */
36
37/** Constructor.
38 * @param varname variable name
39 * @param interface Fawkes interface instance
40 * @param proxy Player proxy instance
41 */
42PlayerLaserMapper::PlayerLaserMapper(const std::string & varname,
44 PlayerCc::LaserProxy * proxy)
46 interface_(interface),
47 proxy_(proxy),
48 first_read_(true),
49 index_offset_(0)
50{
51}
52
53void
55{
56 //printf("Laser interface, count: %u, min angle= %f, max angle = %f\n",
57 // proxy_->GetCount(), proxy_->GetMinAngle(), proxy_->GetMaxAngle());
58
59 if (proxy_->GetCount() != 360)
60 return;
61
62 if (proxy_->IsFresh()) {
63 if (first_read_) {
64 index_offset_ = 360 + fawkes::rad2deg(proxy_->GetMinAngle());
65 first_read_ = false;
66 }
67
68 //printf("Setting %s to (%f, %f, %f)\n", varname().c_str(), proxy_->GetXPos(),
69 // proxy_->GetYPos(), proxy_->GetYaw());
70 float distances[360];
71 for (int i = 0; i < 360; ++i) {
72 distances[(i + index_offset_) % 360] = (*proxy_)[360 - i];
73 }
74 interface_->set_distances(distances);
75 interface_->write();
76 proxy_->NotFresh();
77 }
78}
79
80void
82{
83}
virtual void sync_fawkes_to_player()
Sync Fawkes interface to Player proxy.
virtual void sync_player_to_fawkes()
Sync Player proxy to Fawkes interface.
PlayerLaserMapper(const std::string &varname, fawkes::Laser360Interface *interface, PlayerCc::LaserProxy *proxy)
Constructor.
Player proxy to Fawkes interface mapper interface.
Definition: mapper.h:29
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
Laser360Interface Fawkes BlackBoard Interface.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46