Skip to content

Commit d4139b7

Browse files
Wiktor-99Wiktor-99
authored
Add GPS semantic component (#2000)
1 parent 54a9226 commit d4139b7

File tree

3 files changed

+381
-0
lines changed

3 files changed

+381
-0
lines changed

‎controller_interface/CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,12 @@ if(BUILD_TESTING)
8686
ament_target_dependencies(test_pose_sensor
8787
geometry_msgs
8888
)
89+
90+
ament_add_gmock(test_gps_sensor test/test_gps_sensor.cpp)
91+
target_link_libraries(test_gps_sensor
92+
controller_interface
93+
hardware_interface::hardware_interface
94+
)
8995
endif()
9096

9197
install(

‎controller_interface/include/semantic_components/gps_sensor.hpp

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
// Copyright 2025 ros2_control development team
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
16+
#define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
17+
18+
#include <array>
19+
#include <string>
20+
21+
#include "semantic_components/semantic_component_interface.hpp"
22+
#include "sensor_msgs/msg/nav_sat_fix.hpp"
23+
24+
namespace semantic_components
25+
{
26+
27+
enum class GPSSensorOption
28+
{
29+
WithCovariance,
30+
WithoutCovariance
31+
};
32+
33+
template <GPSSensorOption sensor_option>
34+
class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
35+
{
36+
public:
37+
static_assert(
38+
sensor_option == GPSSensorOption::WithCovariance ||
39+
sensor_option == GPSSensorOption::WithoutCovariance,
40+
"Invalid GPSSensorOption");
41+
explicit GPSSensor(const std::string & name)
42+
: SemanticComponentInterface(
43+
name, {{name + "/" + "status"},
44+
{name + "/" + "service"},
45+
{name + "/" + "latitude"},
46+
{name + "/" + "longitude"},
47+
{name + "/" + "altitude"}})
48+
{
49+
if constexpr (sensor_option == GPSSensorOption::WithCovariance)
50+
{
51+
interface_names_.emplace_back(name + "/" + "latitude_covariance");
52+
interface_names_.emplace_back(name + "/" + "longitude_covariance");
53+
interface_names_.emplace_back(name + "/" + "altitude_covariance");
54+
}
55+
}
56+
57+
/**
58+
* Return GPS's status e.g. fix/no_fix
59+
*
60+
* \return Status
61+
*/
62+
int8_t get_status() const { return static_cast<int8_t>(state_interfaces_[0].get().get_value()); }
63+
64+
/**
65+
* Return service used by GPS e.g. fix/no_fix
66+
*
67+
* \return Service
68+
*/
69+
uint16_t get_service() const
70+
{
71+
return static_cast<uint16_t>(state_interfaces_[1].get().get_value());
72+
}
73+
74+
/**
75+
* Return latitude reported by a GPS
76+
*
77+
* \return Latitude.
78+
*/
79+
double get_latitude() const { return state_interfaces_[2].get().get_value(); }
80+
81+
/**
82+
* Return longitude reported by a GPS
83+
*
84+
* \return Longitude.
85+
*/
86+
double get_longitude() const { return state_interfaces_[3].get().get_value(); }
87+
88+
/**
89+
* Return altitude reported by a GPS
90+
*
91+
* \return Altitude.
92+
*/
93+
double get_altitude() const { return state_interfaces_[4].get().get_value(); }
94+
95+
/**
96+
* Return covariance reported by a GPS.
97+
*
98+
* \return Covariance array.
99+
*/
100+
template <
101+
typename U = void,
102+
typename = std::enable_if_t<sensor_option == GPSSensorOption::WithCovariance, U>>
103+
const std::array<double, 9> & get_covariance()
104+
{
105+
covariance_[0] = state_interfaces_[5].get().get_value();
106+
covariance_[4] = state_interfaces_[6].get().get_value();
107+
covariance_[8] = state_interfaces_[7].get().get_value();
108+
return covariance_;
109+
}
110+
111+
/**
112+
* Fills a NavSatFix message from the current values.
113+
*/
114+
bool get_values_as_message(sensor_msgs::msg::NavSatFix & message)
115+
{
116+
message.status.status = get_status();
117+
message.status.service = get_service();
118+
message.latitude = get_latitude();
119+
message.longitude = get_longitude();
120+
message.altitude = get_altitude();
121+
122+
if constexpr (sensor_option == GPSSensorOption::WithCovariance)
123+
{
124+
message.position_covariance = get_covariance();
125+
}
126+
127+
return true;
128+
}
129+
130+
private:
131+
std::array<double, 9> covariance_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
132+
};
133+
134+
} // namespace semantic_components
135+
136+
#endif // SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_

‎controller_interface/test/test_gps_sensor.cpp

Lines changed: 239 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,239 @@
1+
// Copyright 2021 ros2_control development team
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <gmock/gmock.h>
16+
#include <gtest/gtest.h>
17+
#include <algorithm>
18+
#include <array>
19+
#include <string>
20+
#include <vector>
21+
#include "hardware_interface/handle.hpp"
22+
#include "hardware_interface/loaned_state_interface.hpp"
23+
#include "semantic_components/gps_sensor.hpp"
24+
#include "sensor_msgs/msg/nav_sat_fix.hpp"
25+
26+
struct GPSSensorTest : public testing::Test
27+
{
28+
GPSSensorTest()
29+
{
30+
std::transform(
31+
gps_interface_names.cbegin(), gps_interface_names.cend(),
32+
std::back_inserter(full_interface_names),
33+
[this](const auto & interface_name) { return gps_sensor_name + '/' + interface_name; });
34+
state_interface.emplace_back(gps_state);
35+
state_interface.emplace_back(gps_service);
36+
state_interface.emplace_back(latitude);
37+
state_interface.emplace_back(longitude);
38+
state_interface.emplace_back(altitude);
39+
}
40+
41+
const std::string gps_sensor_name{"gps_sensor"};
42+
const std::array<std::string, 5> gps_interface_names{
43+
{"status", "service", "latitude", "longitude", "altitude"}};
44+
std::array<double, 5> gps_states{};
45+
semantic_components::GPSSensor<semantic_components::GPSSensorOption::WithoutCovariance> sut{
46+
gps_sensor_name};
47+
std::vector<std::string> full_interface_names;
48+
49+
hardware_interface::StateInterface gps_state{
50+
gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)};
51+
hardware_interface::StateInterface gps_service{
52+
gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
53+
hardware_interface::StateInterface latitude{
54+
gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
55+
hardware_interface::StateInterface longitude{
56+
gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
57+
hardware_interface::StateInterface altitude{
58+
gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)};
59+
std::vector<hardware_interface::LoanedStateInterface> state_interface;
60+
};
61+
62+
TEST_F(
63+
GPSSensorTest,
64+
interfaces_name_should_contain_status_latitude_longitude_altitude_and_sensor_name_prefix)
65+
{
66+
const auto senors_interfaces_name = sut.get_state_interface_names();
67+
EXPECT_THAT(senors_interfaces_name, testing::ElementsAreArray(full_interface_names));
68+
}
69+
70+
TEST_F(
71+
GPSSensorTest,
72+
status_latitude_longitude_altitude_should_be_equal_to_corresponding_values_in_state_interface)
73+
{
74+
EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
75+
EXPECT_EQ(gps_states.at(0), sut.get_status());
76+
EXPECT_EQ(gps_states.at(1), sut.get_service());
77+
EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
78+
EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
79+
EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
80+
81+
gps_states.at(0) = 1.0;
82+
gps_states.at(1) = 3.0;
83+
gps_states.at(2) = 2.0;
84+
gps_states.at(3) = 3.0;
85+
gps_states.at(4) = 4.0;
86+
87+
EXPECT_EQ(gps_states.at(0), sut.get_status());
88+
EXPECT_EQ(gps_states.at(1), sut.get_service());
89+
EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
90+
EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
91+
EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
92+
}
93+
94+
TEST_F(GPSSensorTest, should_fill_gps_nav_sat_fix_msg_with_value_from_state_interface)
95+
{
96+
EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
97+
98+
sensor_msgs::msg::NavSatFix message;
99+
EXPECT_TRUE(sut.get_values_as_message(message));
100+
EXPECT_EQ(gps_states.at(0), message.status.status);
101+
EXPECT_EQ(gps_states.at(1), message.status.service);
102+
EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
103+
EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
104+
EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
105+
106+
gps_states.at(0) = 1.0;
107+
gps_states.at(1) = 3.0;
108+
gps_states.at(2) = 2.0;
109+
gps_states.at(3) = 3.0;
110+
gps_states.at(4) = 4.0;
111+
112+
EXPECT_TRUE(sut.get_values_as_message(message));
113+
EXPECT_EQ(gps_states.at(0), message.status.status);
114+
EXPECT_EQ(gps_states.at(1), message.status.service);
115+
EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
116+
EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
117+
EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
118+
}
119+
120+
struct GPSSensorWithCovarianceTest : public testing::Test
121+
{
122+
GPSSensorWithCovarianceTest()
123+
{
124+
std::transform(
125+
gps_interface_names.cbegin(), gps_interface_names.cend(),
126+
std::back_inserter(full_interface_names),
127+
[this](const auto & interface_name) { return gps_sensor_name + '/' + interface_name; });
128+
state_interface.emplace_back(gps_state);
129+
state_interface.emplace_back(gps_service);
130+
state_interface.emplace_back(latitude);
131+
state_interface.emplace_back(longitude);
132+
state_interface.emplace_back(altitude);
133+
state_interface.emplace_back(latitude_covariance);
134+
state_interface.emplace_back(longitude_covariance);
135+
state_interface.emplace_back(altitude_covariance);
136+
}
137+
138+
const std::string gps_sensor_name{"gps_sensor"};
139+
const std::array<std::string, 8> gps_interface_names{
140+
{"status", "service", "latitude", "longitude", "altitude", "latitude_covariance",
141+
"longitude_covariance", "altitude_covariance"}};
142+
std::array<double, 8> gps_states{};
143+
semantic_components::GPSSensor<semantic_components::GPSSensorOption::WithCovariance> sut{
144+
gps_sensor_name};
145+
std::vector<std::string> full_interface_names;
146+
147+
hardware_interface::StateInterface gps_state{
148+
gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)};
149+
hardware_interface::StateInterface gps_service{
150+
gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
151+
hardware_interface::StateInterface latitude{
152+
gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
153+
hardware_interface::StateInterface longitude{
154+
gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
155+
hardware_interface::StateInterface altitude{
156+
gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)};
157+
hardware_interface::StateInterface latitude_covariance{
158+
gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5)};
159+
hardware_interface::StateInterface longitude_covariance{
160+
gps_sensor_name, gps_interface_names.at(6), &gps_states.at(6)};
161+
hardware_interface::StateInterface altitude_covariance{
162+
gps_sensor_name, gps_interface_names.at(7), &gps_states.at(7)};
163+
std::vector<hardware_interface::LoanedStateInterface> state_interface;
164+
};
165+
166+
TEST_F(
167+
GPSSensorWithCovarianceTest,
168+
interfaces_name_should_contain_status_latitude_longitude_altitude_and_sensor_name_prefix)
169+
{
170+
const auto senors_interfaces_name = sut.get_state_interface_names();
171+
172+
EXPECT_EQ(senors_interfaces_name.size(), full_interface_names.size());
173+
EXPECT_THAT(senors_interfaces_name, testing::ElementsAreArray(full_interface_names));
174+
}
175+
176+
TEST_F(
177+
GPSSensorWithCovarianceTest,
178+
status_latitude_longitude_altitude_should_be_equal_to_corresponding_values_in_state_interface)
179+
{
180+
EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
181+
EXPECT_EQ(gps_states.at(0), sut.get_status());
182+
EXPECT_EQ(gps_states.at(1), sut.get_service());
183+
EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
184+
EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
185+
EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
186+
EXPECT_THAT(
187+
sut.get_covariance(), testing::ElementsAreArray({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
188+
189+
gps_states.at(0) = 1.0;
190+
gps_states.at(1) = 3.0;
191+
gps_states.at(2) = 2.0;
192+
gps_states.at(3) = 3.0;
193+
gps_states.at(4) = 4.0;
194+
gps_states.at(5) = 0.5;
195+
gps_states.at(6) = 0.2;
196+
gps_states.at(7) = 0.7;
197+
198+
EXPECT_EQ(gps_states.at(0), sut.get_status());
199+
EXPECT_EQ(gps_states.at(1), sut.get_service());
200+
EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
201+
EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
202+
EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
203+
EXPECT_THAT(
204+
sut.get_covariance(), testing::ElementsAreArray({0.5, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.7}));
205+
}
206+
207+
TEST_F(GPSSensorWithCovarianceTest, should_fill_gps_nav_sat_fix_msg_with_value_from_state_interface)
208+
{
209+
EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
210+
sensor_msgs::msg::NavSatFix message;
211+
EXPECT_TRUE(sut.get_values_as_message(message));
212+
EXPECT_EQ(gps_states.at(0), message.status.status);
213+
EXPECT_EQ(gps_states.at(1), message.status.service);
214+
EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
215+
EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
216+
EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
217+
EXPECT_THAT(
218+
message.position_covariance,
219+
testing::ElementsAreArray({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
220+
221+
gps_states.at(0) = 1.0;
222+
gps_states.at(1) = 2.0;
223+
gps_states.at(2) = 2.0;
224+
gps_states.at(3) = 3.0;
225+
gps_states.at(4) = 4.0;
226+
gps_states.at(5) = 0.5;
227+
gps_states.at(6) = 0.2;
228+
gps_states.at(7) = 0.7;
229+
230+
EXPECT_TRUE(sut.get_values_as_message(message));
231+
EXPECT_EQ(gps_states.at(0), message.status.status);
232+
EXPECT_EQ(gps_states.at(1), message.status.service);
233+
EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
234+
EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
235+
EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
236+
EXPECT_THAT(
237+
message.position_covariance,
238+
testing::ElementsAreArray({0.5, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.7}));
239+
}

0 commit comments

Comments
 (0)