Skip to content

Commit f9b2a69

Browse files
norrocottsayralph-lange
authored
Dev (#20)
* Refactors state and mode pair to struct (#16) * Adds 'how to apply' section to doc (#18): Documents how to design (and how not to design) system modes * Tests for mode inference * Use ament_target_dependencies to link boost_program_options (#17) find_package(Boost) may pass file paths for Boost libraries which are not in the default search locations at link time. Since FindBoost.cmake sets Boost_<C>_FOUND and Boost_<C>_LIBRARIES for each requested component, we can let ament_target_dependencies discover the component as if it were a standalone package. Signed-off-by: Arne Nordmann <arne.nordmann@de.bosch.com> Co-authored-by: Scott K Logan <logans@cottsay.net> Co-authored-by: Ralph Lange <ralph-lange@users.noreply.github.com>
1 parent 734b9b5 commit f9b2a69

16 files changed

Lines changed: 342 additions & 131 deletions

‎system_modes/CMakeLists.txt‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
1616
endif()
1717

1818
# find dependencies
19-
find_package(ament_cmake_ros REQUIRED)
19+
find_package(ament_cmake REQUIRED)
2020
find_package(builtin_interfaces REQUIRED)
2121
find_package(std_msgs REQUIRED)
2222
find_package(rclcpp REQUIRED)
@@ -38,7 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
3838
DEPENDENCIES builtin_interfaces
3939
)
4040

41-
add_library(mode
41+
add_library(mode SHARED
4242
src/system_modes/mode.cpp
4343
src/system_modes/mode_impl.cpp
4444
src/system_modes/mode_inference.cpp)
@@ -133,6 +133,16 @@ if(BUILD_TESTING)
133133
)
134134
target_link_libraries(test_mode mode)
135135
endif()
136+
137+
message(STATUS "CMAKE_CURRENT_BINARY_DIR: ${CMAKE_CURRENT_BINARY_DIR}/system_modes/")
138+
ament_add_gtest(test_mode_inference test/test_mode_inference.cpp)
139+
if(TARGET test_mode_inference)
140+
target_include_directories(test_mode_inference PUBLIC
141+
${rclcpp_INCLUDE_DIRS}
142+
${CMAKE_CURRENT_BINARY_DIR}/system_modes/
143+
)
144+
target_link_libraries(test_mode_inference mode)
145+
endif()
136146
endif()
137147

138148
ament_export_include_directories(include)

‎system_modes/include/system_modes/mode.hpp‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,6 @@ using DefaultModePtr = std::shared_ptr<DefaultMode>;
4141

4242
using ModeMap = std::map<std::string, ModeConstPtr>;
4343

44-
static const char DEFAULT_MODE[] = "__DEFAULT__";
45-
4644
class ModeBase
4745
{
4846
public:
@@ -56,14 +54,14 @@ class ModeBase
5654
virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
5755
virtual void set_part_mode(
5856
const std::string & part,
59-
const std::pair<unsigned int, std::string> stateAndMode) = 0;
57+
const StateAndMode stateAndMode) = 0;
6058

6159
virtual rclcpp::Parameter get_parameter(const std::string & param_name) const;
6260
virtual std::vector<std::string> get_parameter_names() const;
6361
virtual const std::vector<rclcpp::Parameter> get_parameters() const;
6462

6563
virtual const std::vector<std::string> get_parts() const;
66-
virtual const std::pair<unsigned int, std::string> get_part_mode(const std::string & part) const;
64+
virtual const StateAndMode get_part_mode(const std::string & part) const;
6765

6866
virtual std::string print() const;
6967

@@ -83,7 +81,7 @@ class DefaultMode : public ModeBase
8381

8482
virtual void set_part_mode(
8583
const std::string & part,
86-
const std::pair<unsigned int, std::string> stateAndMode);
84+
const StateAndMode stateAndMode);
8785
};
8886

8987
class Mode : public ModeBase
@@ -100,7 +98,7 @@ class Mode : public ModeBase
10098

10199
virtual void set_part_mode(
102100
const std::string & part,
103-
const std::pair<unsigned int, std::string> stateAndMode);
101+
const StateAndMode stateAndMode);
104102
};
105103

106104
inline std::ostream & operator<<(std::ostream & os, const Mode & mode)

‎system_modes/include/system_modes/mode_impl.hpp‎

Lines changed: 24 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,26 @@
2727
namespace system_modes
2828
{
2929

30+
static const char DEFAULT_MODE[] = "__DEFAULT__";
31+
32+
struct StateAndMode {
33+
unsigned int state;
34+
std::string mode;
35+
36+
StateAndMode(unsigned int newstate = 0, const std::string& newmode = "")
37+
{
38+
state = newstate;
39+
mode = newmode;
40+
}
41+
bool operator!=(const StateAndMode& cmp) const
42+
{
43+
return (cmp.state == state // same state
44+
&& (cmp.mode.compare(mode) == 0 // same mode
45+
|| (cmp.mode.compare(DEFAULT_MODE) == 0 && mode.empty()) // we consider empty and
46+
|| (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty()))); // DEFAULT_MODE the same
47+
}
48+
};
49+
3050
class ModeImpl
3151
{
3252
public:
@@ -40,26 +60,26 @@ class ModeImpl
4060
virtual void add_parameters(const std::vector<rclcpp::Parameter> & parameters);
4161
virtual void add_part_mode(
4262
const std::string & part,
43-
const std::pair<unsigned int, std::string> stateAndMode);
63+
const StateAndMode stateAndMode);
4464

4565
virtual void set_parameter(const rclcpp::Parameter & parameter);
4666
virtual void set_parameters(const std::vector<rclcpp::Parameter> & parameters);
4767
virtual void set_part_mode(
4868
const std::string & part,
49-
const std::pair<unsigned int, std::string> stateAndMode);
69+
const StateAndMode stateAndMode);
5070

5171
virtual std::vector<std::string> get_parameter_names() const;
5272
virtual rclcpp::Parameter get_parameter(const std::string & param_name) const;
5373
virtual bool get_parameter(const std::string & param_name, rclcpp::Parameter & parameter) const;
5474
virtual const std::vector<rclcpp::Parameter> get_parameters() const;
5575

5676
virtual const std::vector<std::string> get_parts() const;
57-
virtual const std::pair<unsigned int, std::string> get_part_mode(const std::string & part) const;
77+
virtual const StateAndMode get_part_mode(const std::string & part) const;
5878

5979
protected:
6080
std::string name_;
6181
std::map<std::string, rclcpp::Parameter> param_;
62-
std::map<std::string, std::pair<unsigned int, std::string>> part_modes_;
82+
std::map<std::string, StateAndMode> part_modes_;
6383
mutable std::mutex mutex_;
6484
};
6585

‎system_modes/include/system_modes/mode_inference.hpp‎

Lines changed: 15 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,10 @@
3131
namespace system_modes
3232
{
3333

34+
typedef std::map<std::string, StateAndMode > StatesMap;
35+
typedef std::map<std::string, std::map<std::string, rclcpp::Parameter> > ParametersMap;
36+
typedef std::map<std::string, std::pair<StateAndMode, StateAndMode > > Deviation;
37+
3438
class ModeInference
3539
{
3640
public:
@@ -43,19 +47,20 @@ class ModeInference
4347
virtual const std::vector<std::string> get_all_parts_of(
4448
const std::string & system) const;
4549

50+
virtual void update(const std::string &, const StateAndMode &);
4651
virtual void update_state(const std::string &, unsigned int);
4752
virtual void update_mode(const std::string &, const std::string &);
4853
virtual void update_param(const std::string &, rclcpp::Parameter &);
49-
virtual void update_target(const std::string &, std::pair<unsigned int, std::string>);
54+
virtual void update_target(const std::string &, StateAndMode);
5055

51-
virtual std::pair<unsigned int, std::string> get(const std::string & part);
52-
virtual std::pair<unsigned int, std::string> get_or_infer(const std::string & part);
56+
virtual StateAndMode get(const std::string & part);
57+
virtual StateAndMode get_or_infer(const std::string & part);
5358

54-
virtual std::pair<unsigned int, std::string> infer(const std::string & part);
55-
virtual std::pair<unsigned int, std::string> infer_system(const std::string & part);
56-
virtual std::pair<unsigned int, std::string> infer_node(const std::string & part);
59+
virtual StateAndMode infer(const std::string & part);
60+
virtual StateAndMode infer_system(const std::string & part);
61+
virtual StateAndMode infer_node(const std::string & part);
5762

58-
virtual std::pair<unsigned int, std::string> get_target(const std::string & part);
63+
virtual StateAndMode get_target(const std::string & part);
5964
virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode);
6065
virtual std::vector<std::string> get_available_modes(const std::string & part);
6166

@@ -67,10 +72,10 @@ class ModeInference
6772
virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &);
6873

6974
private:
70-
std::map<std::string, std::pair<unsigned int, std::string>> nodes_, nodes_target_;
71-
std::map<std::string, std::pair<unsigned int, std::string>> systems_, systems_target_;
75+
StatesMap nodes_, nodes_target_;
76+
StatesMap systems_, systems_target_;
7277
std::map<std::string, ModeMap> modes_;
73-
std::map<std::string, std::map<std::string, rclcpp::Parameter>> parameters_;
78+
ParametersMap parameters_;
7479

7580
mutable std::shared_timed_mutex
7681
nodes_mutex_, systems_mutex_,

‎system_modes/package.xml‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,16 +7,15 @@
77
<maintainer email="arne.nordmann@bosch.com">Arne Nordmann</maintainer>
88
<license>Apache License 2.0</license>
99

10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
1012
<depend>rclcpp</depend>
1113
<depend>rclcpp_lifecycle</depend>
1214
<depend>std_msgs</depend>
1315
<depend>builtin_interfaces</depend>
1416
<depend>rosidl_default_generators</depend>
1517
<depend>libboost-program-options-dev</depend>
1618

17-
<buildtool_depend>ament_cmake_ros</buildtool_depend>
18-
19-
<test_depend>ament_cmake</test_depend>
2019
<test_depend>ament_cmake_gtest</test_depend>
2120
<test_depend>ament_cmake_gmock</test_depend>
2221
<test_depend>ament_cmake_pep257</test_depend>

‎system_modes/src/mode_manager_node.cpp‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ using std::shared_ptr;
4040

4141
using system_modes::ModeManager;
4242
using system_modes::DEFAULT_MODE;
43+
using system_modes::StateAndMode;
4344
using system_modes::msg::ModeEvent;
4445

4546
using lifecycle_msgs::msg::State;
@@ -108,11 +109,11 @@ void transition_request_callback(
108109
if (msg->goal_state.id != State::PRIMARY_STATE_ACTIVE) {
109110
manager->inference()->update_target(
110111
node_name,
111-
make_pair(msg->goal_state.id, ""));
112+
StateAndMode(msg->goal_state.id, ""));
112113
} else {
113114
manager->inference()->update_target(
114115
node_name,
115-
make_pair(msg->goal_state.id, DEFAULT_MODE));
116+
StateAndMode(msg->goal_state.id, DEFAULT_MODE));
116117
}
117118
}
118119

@@ -122,7 +123,7 @@ void mode_request_callback(
122123
{
123124
manager->inference()->update_target(
124125
node_name,
125-
make_pair(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
126+
StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
126127
}
127128

128129
void

‎system_modes/src/mode_monitor_node.cpp‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ using std::invalid_argument;
4444

4545
using system_modes::ModeMonitor;
4646
using system_modes::DEFAULT_MODE;
47+
using system_modes::StateAndMode;
4748
using system_modes::msg::ModeEvent;
4849
using lifecycle_msgs::msg::State;
4950
using lifecycle_msgs::msg::TransitionEvent;
@@ -130,11 +131,11 @@ void transition_request_callback(
130131
if (msg->goal_state.id != State::PRIMARY_STATE_ACTIVE) {
131132
monitor->inference()->update_target(
132133
node_name,
133-
make_pair(msg->goal_state.id, ""));
134+
StateAndMode(msg->goal_state.id, ""));
134135
} else {
135136
monitor->inference()->update_target(
136137
node_name,
137-
make_pair(msg->goal_state.id, DEFAULT_MODE));
138+
StateAndMode(msg->goal_state.id, DEFAULT_MODE));
138139
}
139140
}
140141

@@ -144,7 +145,7 @@ void mode_request_callback(
144145
{
145146
monitor->inference()->update_target(
146147
node_name,
147-
make_pair(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
148+
StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
148149
}
149150

150151
void

‎system_modes/src/system_modes/mode.cpp‎

Lines changed: 8 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,6 @@
2222
#include <vector>
2323
#include <utility>
2424

25-
using std::pair;
2625
using std::string;
2726
using std::vector;
2827
using std::to_string;
@@ -70,7 +69,7 @@ ModeBase::get_parts() const
7069
return this->mode_impl_.get_parts();
7170
}
7271

73-
const pair<unsigned int, string>
72+
const StateAndMode
7473
ModeBase::get_part_mode(const string & part) const
7574
{
7675
return this->mode_impl_.get_part_mode(part);
@@ -100,7 +99,7 @@ ModeBase::print() const
10099
first = false;
101100
}
102101
auto stateAndMode = this->get_part_mode(p);
103-
os << p << ":" << stateAndMode.first << stateAndMode.second;
102+
os << p << ":" << stateAndMode.state << stateAndMode.mode;
104103
}
105104

106105
os << ">";
@@ -127,10 +126,10 @@ DefaultMode::set_parameters(const vector<Parameter> & parameters)
127126
void
128127
DefaultMode::set_part_mode(
129128
const std::string & part,
130-
const std::pair<unsigned int, std::string> stateAndMode)
129+
const StateAndMode stateAndMode)
131130
{
132-
if (stateAndMode.second.empty()) {
133-
this->mode_impl_.add_part_mode(part, std::make_pair(stateAndMode.first, DEFAULT_MODE));
131+
if (stateAndMode.mode.empty()) {
132+
this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, DEFAULT_MODE));
134133
} else {
135134
this->mode_impl_.add_part_mode(part, stateAndMode);
136135
}
@@ -169,10 +168,10 @@ Mode::set_parameters(const vector<Parameter> & parameters)
169168
void
170169
Mode::set_part_mode(
171170
const std::string & part,
172-
const std::pair<unsigned int, std::string> stateAndMode)
171+
const StateAndMode stateAndMode)
173172
{
174-
if (stateAndMode.second.empty()) {
175-
this->mode_impl_.add_part_mode(part, std::make_pair(stateAndMode.first, DEFAULT_MODE));
173+
if (stateAndMode.mode.empty()) {
174+
this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, DEFAULT_MODE));
176175
} else {
177176
this->mode_impl_.add_part_mode(part, stateAndMode);
178177
}

‎system_modes/src/system_modes/mode_impl.cpp‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -137,15 +137,15 @@ ModeImpl::set_parameters(const vector<Parameter> & parameters)
137137
void
138138
ModeImpl::add_part_mode(
139139
const std::string & part,
140-
const std::pair<unsigned int, std::string> stateAndMode)
140+
const StateAndMode stateAndMode)
141141
{
142-
this->part_modes_[part] = stateAndMode;
142+
this->part_modes_[part] = {stateAndMode.state, stateAndMode.mode};
143143
}
144144

145145
void
146146
ModeImpl::set_part_mode(
147147
const std::string & part,
148-
const std::pair<unsigned int, std::string> stateAndMode)
148+
const StateAndMode stateAndMode)
149149
{
150150
if (this->part_modes_.find(part) == this->part_modes_.end()) {
151151
throw out_of_range(
@@ -166,13 +166,15 @@ ModeImpl::get_parts() const
166166
return results;
167167
}
168168

169-
const std::pair<unsigned int, std::string>
169+
const StateAndMode
170170
ModeImpl::get_part_mode(const std::string & part) const
171171
{
172172
if (this->part_modes_.count(part)) {
173173
return this->part_modes_.at(part);
174174
} else {
175-
return std::make_pair(0, "");
175+
throw std::out_of_range(
176+
"Can't receive modes for part '" + part +
177+
"', part not specified.");
176178
}
177179
}
178180

0 commit comments

Comments
 (0)