Skip to content

Commit 2d0b6d7

Browse files
ButakusButakus
committed
Update plugins to new sensors API
1 parent af06ef8 commit 2d0b6d7

18 files changed

Lines changed: 49 additions & 45 deletions

File tree

‎controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@
3737

3838
#include "easynav_core/ControllerMethodBase.hpp"
3939
#include "easynav_common/types/NavState.hpp"
40-
#include "easynav_common/types/PointPerception.hpp"
40+
#include "easynav_sensors/types/PointPerception.hpp"
4141

4242
#include "easynav_mpc_controller/MPCOptimizer.hpp"
4343

‎controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -142,7 +142,7 @@ MPCController::update_rt(NavState & nav_state)
142142
}
143143
}
144144

145-
if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("points")) {
145+
if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has_group("points")) {
146146
if(verbose_) {
147147
std::cout << "No Path, No Points or No Robot Pose" << std::endl;
148148
}
@@ -209,7 +209,7 @@ MPCController::update_rt(NavState & nav_state)
209209
}
210210
const auto & last_pose = path.poses[local_horizon].pose.position;
211211

212-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
212+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
213213
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
214214

215215
const auto & filtered = PointPerceptionsOpsView(perceptions)

‎controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
/// \brief Implementation of the MPPIController class.
1818

1919
#include "easynav_mppi_controller/MPPIController.hpp"
20-
#include "easynav_common/types/PointPerception.hpp"
20+
#include "easynav_sensors/types/PointPerception.hpp"
2121
#include "easynav_common/RTTFBuffer.hpp"
2222

2323
#include "easynav_system/GoalManager.hpp"
@@ -182,7 +182,7 @@ MPPIController::update_rt(NavState & nav_state)
182182
}
183183

184184
const auto & pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
185-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
185+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
186186
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
187187
const auto & filtered = PointPerceptionsOpsView(perceptions)
188188
.filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0})

‎controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323
#include "tf2/utils.hpp"
2424
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
2525

26-
#include "easynav_common/types/PointPerception.hpp"
26+
#include "easynav_sensors/types/PointPerception.hpp"
2727
#include "easynav_common/RTTFBuffer.hpp"
2828

2929
#include "easynav_serest_controller/SerestController.hpp"
@@ -295,9 +295,9 @@ SerestController::closest_obstacle_distance(
295295
}
296296

297297
// 2) Analyze distance sensors
298-
if (!nav_state.has("points")) {return std::numeric_limits<double>::infinity();}
298+
if (!nav_state.has_group("points")) {return std::numeric_limits<double>::infinity();}
299299

300-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
300+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
301301
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
302302

303303
auto view = PointPerceptionsOpsView(perceptions);

‎controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818

1919
#include "easynav_vff_controller/VffController.hpp"
2020
#include "easynav_common/types/NavState.hpp"
21-
#include "easynav_common/types/PointPerception.hpp"
21+
#include "easynav_sensors/types/PointPerception.hpp"
2222

2323
#include "nav_msgs/msg/odometry.hpp"
2424
#include "nav_msgs/msg/goals.hpp"
@@ -252,7 +252,7 @@ void VffController::update_rt(NavState & nav_state)
252252
// Calculate the angle error
253253
double angle_error = normalize_angle(bearing - yaw);
254254

255-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
255+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
256256

257257
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
258258
auto fused =

‎localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222
#include "tf2/LinearMath/Vector3.hpp"
2323

2424
#include "easynav_common/RTTFBuffer.hpp"
25-
#include "easynav_common/types/PointPerception.hpp"
25+
#include "easynav_sensors/types/PointPerception.hpp"
2626
#include "easynav_costmap_common/costmap_2d.hpp"
2727
#include "easynav_costmap_common/cost_values.hpp"
2828

@@ -566,12 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state)
566566
void
567567
AMCLLocalizer::correct(NavState & nav_state)
568568
{
569-
if (!nav_state.has("points")) {
569+
if (!nav_state.has_group("points")) {
570570
RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions");
571571
return;
572572
}
573573

574-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
574+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
575575

576576
if (!nav_state.has("map.static")) {
577577
RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map");

‎localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@
44

55
#include "easynav_common/RTTFBuffer.hpp"
66

7-
#include "easynav_common/types/IMUPerception.hpp"
8-
#include "easynav_common/types/GNSSPerception.hpp"
7+
#include "easynav_sensors/types/IMUPerception.hpp"
8+
#include "easynav_sensors/types/GNSSPerception.hpp"
99
#include "sensor_msgs/msg/nav_sat_status.hpp"
1010

1111
#include <GeographicLib/UTMUPS.hpp>

‎localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,8 @@
3434
#include "tf2/LinearMath/Vector3.hpp"
3535

3636
#include "easynav_common/RTTFBuffer.hpp"
37-
#include "easynav_common/types/PointPerception.hpp"
38-
#include "easynav_common/types/IMUPerception.hpp"
37+
#include "easynav_sensors/types/PointPerception.hpp"
38+
#include "easynav_sensors/types/IMUPerception.hpp"
3939

4040
#include "navmap_core/NavMap.hpp"
4141

@@ -689,11 +689,11 @@ static ScoreAgg score_particle_sensor_cloud(
689689
// ---------- correct() ----------
690690
void AMCLLocalizer::correct(NavState & nav_state)
691691
{
692-
if (!nav_state.has("points")) {
692+
if (!nav_state.has_group("points")) {
693693
RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet");
694694
return;
695695
}
696-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
696+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
697697

698698
if (!nav_state.has("map.bonxai")) {
699699
RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet");

‎localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222
#include "tf2/LinearMath/Vector3.hpp"
2323

2424
#include "easynav_common/RTTFBuffer.hpp"
25-
#include "easynav_common/types/PointPerception.hpp"
25+
#include "easynav_sensors/types/PointPerception.hpp"
2626
#include "easynav_simple_common/SimpleMap.hpp"
2727

2828
#include "easynav_simple_localizer/AMCLLocalizer.hpp"
@@ -368,12 +368,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state)
368368

369369
void AMCLLocalizer::correct(NavState & nav_state)
370370
{
371-
if (!nav_state.has("points")) {
371+
if (!nav_state.has_group("points")) {
372372
RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions");
373373
return;
374374
}
375375

376-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
376+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
377377

378378
if (!nav_state.has("map.static")) {
379379
RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map");

‎maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818

1919
#include "easynav_costmap_common/costmap_2d.hpp"
2020
#include "easynav_common/types/NavState.hpp"
21-
#include "easynav_common/types/PointPerception.hpp"
21+
#include "easynav_sensors/types/PointPerception.hpp"
2222

2323
#include "easynav_costmap_common/costmap_2d.hpp"
2424
#include "easynav_costmap_common/cost_values.hpp"
@@ -41,11 +41,11 @@ ObstacleFilter::on_initialize()
4141
void
4242
ObstacleFilter::update(NavState & nav_state)
4343
{
44-
if (!nav_state.has("points")) {
44+
if (!nav_state.has_group("points")) {
4545
return;
4646
}
4747

48-
const auto & perceptions = nav_state.get<PointPerceptions>("points");
48+
const auto & perceptions = nav_state.get_group<PointPerception>("points");
4949

5050
auto dynamic_map_ptr = nav_state.get_ptr<Costmap2D>("map.dynamic.filtered");
5151
Costmap2D & dynamic_map = *dynamic_map_ptr;

0 commit comments

Comments
 (0)