Skip to content

Commit 3c5ab7b

Browse files
Bernardo-GabrielBernardo-Gabriel
committed
Merge branch 'feature/custom_message_filter'
2 parents f505275 + ca226cd commit 3c5ab7b

15 files changed

Lines changed: 587 additions & 2 deletions

File tree

‎src/DUNE/Tasks.hpp‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,6 @@ namespace DUNE
4949
#include <DUNE/Tasks/Recipient.hpp>
5050
#include <DUNE/Tasks/AbstractCreator.hpp>
5151
#include <DUNE/Tasks/ParameterTable.hpp>
52-
#include <DUNE/Tasks/SimpleTransport.hpp>
53-
#include <DUNE/Tasks/MessageFilter.hpp>
5452
#include <DUNE/Tasks/SourceFilter.hpp>
5553

5654
#endif

‎src/DUNE/Tasks/CustomMessageFilters/CustomMessageFilter.hpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
//***************************************************************************
2+
// Copyright 2007-2026 Universidade do Porto - Faculdade de Engenharia *
3+
// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) *
4+
//***************************************************************************
5+
// This file is part of DUNE: Unified Navigation Environment. *
6+
// *
7+
// Commercial Licence Usage *
8+
// Licencees holding valid commercial DUNE licences may use this file in *
9+
// accordance with the commercial licence agreement provided with the *
10+
// Software or, alternatively, in accordance with the terms contained in a *
11+
// written agreement between you and Faculdade de Engenharia da *
12+
// Universidade do Porto. For licensing terms, conditions, and further *
13+
// information contact lsts@fe.up.pt. *
14+
// *
15+
// Modified European Union Public Licence - EUPL v.1.1 Usage *
16+
// Alternatively, this file may be used under the terms of the Modified *
17+
// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md *
18+
// included in the packaging of this file. You may not use this work *
19+
// except in compliance with the Licence. Unless required by applicable *
20+
// law or agreed to in writing, software distributed under the Licence is *
21+
// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF *
22+
// ANY KIND, either express or implied. See the Licence for the specific *
23+
// language governing permissions and limitations at *
24+
// https://github.com/LSTS/dune/blob/master/LICENCE.md and *
25+
// http://ec.europa.eu/idabc/eupl.html. *
26+
//***************************************************************************
27+
// Author: Bernardo Gabriel *
28+
//***************************************************************************
29+
30+
#ifndef DUNE_TASKS_CUSTOM_MESSAGE_FILTERS_CUSTOM_MESSAGE_FILTERS_HPP_INCLUDED_
31+
#define DUNE_TASKS_CUSTOM_MESSAGE_FILTERS_CUSTOM_MESSAGE_FILTERS_HPP_INCLUDED_
32+
33+
#include <functional>
34+
35+
#include <DUNE/IMC/Definitions.hpp>
36+
37+
namespace DUNE
38+
{
39+
namespace Tasks
40+
{
41+
class CustomMessageFilter
42+
{
43+
public:
44+
CustomMessageFilter(uint32_t mid):
45+
m_mid(mid)
46+
{ }
47+
48+
virtual ~CustomMessageFilter() = default;
49+
50+
virtual bool
51+
filter(const IMC::Message* msg) = 0;
52+
53+
uint32_t
54+
getMessageId(void) const
55+
{
56+
return m_mid;
57+
}
58+
59+
private:
60+
//! Message id.
61+
uint32_t m_mid;
62+
};
63+
}
64+
}
65+
66+
#endif

‎src/DUNE/Tasks/CustomMessageFilters/EntityStateChangeFilter.hpp‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
//***************************************************************************
2+
// Copyright 2007-2026 Universidade do Porto - Faculdade de Engenharia *
3+
// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) *
4+
//***************************************************************************
5+
// This file is part of DUNE: Unified Navigation Environment. *
6+
// *
7+
// Commercial Licence Usage *
8+
// Licencees holding valid commercial DUNE licences may use this file in *
9+
// accordance with the commercial licence agreement provided with the *
10+
// Software or, alternatively, in accordance with the terms contained in a *
11+
// written agreement between you and Faculdade de Engenharia da *
12+
// Universidade do Porto. For licensing terms, conditions, and further *
13+
// information contact lsts@fe.up.pt. *
14+
// *
15+
// Modified European Union Public Licence - EUPL v.1.1 Usage *
16+
// Alternatively, this file may be used under the terms of the Modified *
17+
// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md *
18+
// included in the packaging of this file. You may not use this work *
19+
// except in compliance with the Licence. Unless required by applicable *
20+
// law or agreed to in writing, software distributed under the Licence is *
21+
// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF *
22+
// ANY KIND, either express or implied. See the Licence for the specific *
23+
// language governing permissions and limitations at *
24+
// https://github.com/LSTS/dune/blob/master/LICENCE.md and *
25+
// http://ec.europa.eu/idabc/eupl.html. *
26+
//***************************************************************************
27+
// Author: Bernardo Gabriel *
28+
//***************************************************************************
29+
30+
#ifndef DUNE_TASKS_CUSTOM_MESSAGE_FILTERS_ENTITY_STATE_CHANGE_FILTER_HPP_INCLUDED_
31+
#define DUNE_TASKS_CUSTOM_MESSAGE_FILTERS_ENTITY_STATE_CHANGE_FILTER_HPP_INCLUDED_
32+
33+
#include "CustomMessageFilter.hpp"
34+
35+
#include <unordered_map>
36+
#include <unordered_set>
37+
38+
#include <DUNE/IMC/Definitions.hpp>
39+
#include <DUNE/Utils/String.hpp>
40+
#include <DUNE/Tasks/Task.hpp>
41+
42+
namespace DUNE
43+
{
44+
namespace Tasks
45+
{
46+
class EntityStateChangeFilter : public CustomMessageFilter
47+
{
48+
public:
49+
EntityStateChangeFilter(DUNE::Tasks::Task* task, const std::string& spec = ""):
50+
CustomMessageFilter(DUNE::IMC::EntityState::getIdStatic())
51+
{
52+
if (spec.empty())
53+
return;
54+
55+
std::vector<std::string> parts;
56+
DUNE::Utils::String::split(spec, "+", parts);
57+
for (auto& part : parts)
58+
{
59+
try
60+
{
61+
auto id = task->resolveEntity(part);
62+
m_monitored_entities.insert(id);
63+
}
64+
catch(const std::exception& e)
65+
{
66+
DUNE_WRN("EntityStateChangeFilter", "invalid entity label in filter specification: " << part);
67+
continue;
68+
}
69+
}
70+
}
71+
72+
bool
73+
filter(const DUNE::IMC::Message* msg) override
74+
{
75+
const auto entity_state_msg = dynamic_cast<const DUNE::IMC::EntityState*>(msg);
76+
if (entity_state_msg == nullptr)
77+
return false;
78+
79+
auto id = entity_state_msg->getSourceEntity();
80+
if (!m_monitored_entities.empty() && m_monitored_entities.find(id) == m_monitored_entities.end())
81+
return false;
82+
83+
auto state = entity_state_msg->state;
84+
auto it = m_entity_states.find(id);
85+
if (it == m_entity_states.end())
86+
{
87+
m_entity_states[id] = state;
88+
return false;
89+
}
90+
91+
if (it->second == state)
92+
return true;
93+
94+
it->second = state;
95+
return false;
96+
}
97+
98+
private:
99+
//! Map of entity id to last known state.
100+
std::unordered_map<uint16_t, uint8_t> m_entity_states;
101+
//! Set of entity ids to monitor.
102+
std::unordered_set<uint16_t> m_monitored_entities;
103+
};
104+
}
105+
}
106+
107+
#endif

‎src/DUNE/Tasks/CustomMessageFilters/Factory.cpp‎

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,133 @@
1+
//***************************************************************************
2+
// Copyright 2007-2026 Universidade do Porto - Faculdade de Engenharia *
3+
// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) *
4+
//***************************************************************************
5+
// This file is part of DUNE: Unified Navigation Environment. *
6+
// *
7+
// Commercial Licence Usage *
8+
// Licencees holding valid commercial DUNE licences may use this file in *
9+
// accordance with the commercial licence agreement provided with the *
10+
// Software or, alternatively, in accordance with the terms contained in a *
11+
// written agreement between you and Faculdade de Engenharia da *
12+
// Universidade do Porto. For licensing terms, conditions, and further *
13+
// information contact lsts@fe.up.pt. *
14+
// *
15+
// Modified European Union Public Licence - EUPL v.1.1 Usage *
16+
// Alternatively, this file may be used under the terms of the Modified *
17+
// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md *
18+
// included in the packaging of this file. You may not use this work *
19+
// except in compliance with the Licence. Unless required by applicable *
20+
// law or agreed to in writing, software distributed under the Licence is *
21+
// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF *
22+
// ANY KIND, either express or implied. See the Licence for the specific *
23+
// language governing permissions and limitations at *
24+
// https://github.com/LSTS/dune/blob/master/LICENCE.md and *
25+
// http://ec.europa.eu/idabc/eupl.html. *
26+
//***************************************************************************
27+
// Author: Bernardo Gabriel *
28+
//***************************************************************************
29+
30+
#include "Factory.hpp"
31+
32+
#include <DUNE/Utils/Utils.hpp>
33+
#include <DUNE/Streams/Terminal.hpp>
34+
#include <DUNE/IMC/Factory.hpp>
35+
36+
#include "EntityStateChangeFilter.hpp"
37+
#include "LogBookEntryFilter.hpp"
38+
39+
namespace DUNE
40+
{
41+
namespace Tasks
42+
{
43+
typedef std::unique_ptr<CustomMessageFilter> (*Creator) (DUNE::Tasks::Task*, const std::string&);
44+
45+
template <typename Type>
46+
static std::unique_ptr<CustomMessageFilter>
47+
create(DUNE::Tasks::Task* task, const std::string& spec)
48+
{
49+
return std::make_unique<Type>(task, spec);
50+
}
51+
52+
static std::pair<uint32_t, std::string> pairs_id_abbrev[] =
53+
{
54+
#define CUSTOM_FILTER(msg_abbrev, filter_abbrev) \
55+
std::pair<uint32_t, std::string>(DUNE::IMC::Factory::getIdFromAbbrev(#msg_abbrev), #filter_abbrev),
56+
#include <DUNE/Tasks/CustomMessageFilters/Factory.def>
57+
};
58+
59+
static std::pair<std::string, uint32_t> pairs_abbrev_id[] =
60+
{
61+
#define CUSTOM_FILTER(msg_abbrev, filter_abbrev) \
62+
std::pair<std::string, uint32_t>(#filter_abbrev, DUNE::IMC::Factory::getIdFromAbbrev(#msg_abbrev)),
63+
#include <DUNE/Tasks/CustomMessageFilters/Factory.def>
64+
};
65+
66+
static std::pair<uint32_t, Creator> creator_pairs_id[] =
67+
{
68+
#define CUSTOM_FILTER(msg_abbrev, filter_abbrev) \
69+
std::pair<uint32_t, Creator>(DUNE::IMC::Factory::getIdFromAbbrev(#msg_abbrev), &create<filter_abbrev>),
70+
#include <DUNE/Tasks/CustomMessageFilters/Factory.def>
71+
};
72+
73+
DUNE_DECLARE_STATIC_MAP(creators_by_id, uint32_t, Creator, creator_pairs_id);
74+
DUNE_DECLARE_STATIC_MAP(map_id_abbrev, uint32_t, std::string, pairs_id_abbrev);
75+
DUNE_DECLARE_STATIC_MAP(map_abbrev_id, std::string, uint32_t, pairs_abbrev_id);
76+
77+
std::string
78+
CustomMessageFilterFactory::getName(uint32_t id)
79+
{
80+
auto it = map_id_abbrev.find(id);
81+
if (it != map_id_abbrev.end())
82+
return it->second;
83+
84+
return "";
85+
}
86+
87+
void
88+
CustomMessageFilterFactory::getNames(std::vector<std::string>& names)
89+
{
90+
names.clear();
91+
for (auto it = map_id_abbrev.begin(); it != map_id_abbrev.end(); ++it)
92+
names.push_back(it->second);
93+
}
94+
95+
void
96+
CustomMessageFilterFactory::getNames(std::string& names)
97+
{
98+
std::vector<std::string> names_vec;
99+
getNames(names_vec);
100+
names = Utils::String::join(names_vec.begin(), names_vec.end(), ",");
101+
}
102+
103+
uint32_t
104+
CustomMessageFilterFactory::getId(const std::string& name)
105+
{
106+
auto it = map_abbrev_id.find(name);
107+
if (it != map_abbrev_id.end())
108+
return it->second;
109+
110+
return 0;
111+
}
112+
113+
std::unique_ptr<CustomMessageFilter>
114+
CustomMessageFilterFactory::produce(DUNE::Tasks::Task* task, const std::string& name, const std::string& spec)
115+
{
116+
auto it = map_abbrev_id.find(name);
117+
if (it != map_abbrev_id.end())
118+
return produce(task, it->second, spec);
119+
120+
return nullptr;
121+
}
122+
123+
std::unique_ptr<CustomMessageFilter>
124+
CustomMessageFilterFactory::produce(DUNE::Tasks::Task* task, uint32_t id, const std::string& spec)
125+
{
126+
if (creators_by_id[id])
127+
return creators_by_id[id](task, spec);
128+
129+
DUNE_DBG("Custom Filter Factory", "unknown custom filter " << id);
130+
return nullptr;
131+
}
132+
}
133+
}

‎src/DUNE/Tasks/CustomMessageFilters/Factory.def‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
//***************************************************************************
2+
// Copyright 2007-2026 Universidade do Porto - Faculdade de Engenharia *
3+
// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) *
4+
//***************************************************************************
5+
// This file is part of DUNE: Unified Navigation Environment. *
6+
// *
7+
// Commercial Licence Usage *
8+
// Licencees holding valid commercial DUNE licences may use this file in *
9+
// accordance with the commercial licence agreement provided with the *
10+
// Software or, alternatively, in accordance with the terms contained in a *
11+
// written agreement between you and Faculdade de Engenharia da *
12+
// Universidade do Porto. For licensing terms, conditions, and further *
13+
// information contact lsts@fe.up.pt. *
14+
// *
15+
// Modified European Union Public Licence - EUPL v.1.1 Usage *
16+
// Alternatively, this file may be used under the terms of the Modified *
17+
// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md *
18+
// included in the packaging of this file. You may not use this work *
19+
// except in compliance with the Licence. Unless required by applicable *
20+
// law or agreed to in writing, software distributed under the Licence is *
21+
// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF *
22+
// ANY KIND, either express or implied. See the Licence for the specific *
23+
// language governing permissions and limitations at *
24+
// https://github.com/LSTS/dune/blob/master/LICENCE.md and *
25+
// http://ec.europa.eu/idabc/eupl.html. *
26+
//***************************************************************************
27+
// Author: Bernardo Gabriel *
28+
//***************************************************************************
29+
30+
CUSTOM_FILTER(EntityState, EntityStateChangeFilter)
31+
CUSTOM_FILTER(LogBookEntry, LogBookEntryFilter)
32+
#undef CUSTOM_FILTER

0 commit comments

Comments
 (0)