Skip to content

Commit 21bef9d

Browse files
fmricofmrico
committed
Adaptations to #94
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
1 parent 2d0b6d7 commit 21bef9d

12 files changed

Lines changed: 39 additions & 35 deletions

File tree

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

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

145-
if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has_group("points")) {
145+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
146+
147+
if (!nav_state.has("path") || !nav_state.has("robot_pose") || perceptions.empty()) {
146148
if(verbose_) {
147149
std::cout << "No Path, No Points or No Robot Pose" << std::endl;
148150
}
@@ -208,8 +210,6 @@ MPCController::update_rt(NavState & nav_state)
208210
local_horizon = num_elements - 1;
209211
}
210212
const auto & last_pose = path.poses[local_horizon].pose.position;
211-
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: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -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_group<PointPerception>("points");
185+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
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: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -297,7 +297,7 @@ SerestController::closest_obstacle_distance(
297297
// 2) Analyze distance sensors
298298
if (!nav_state.has_group("points")) {return std::numeric_limits<double>::infinity();}
299299

300-
const auto & perceptions = nav_state.get_group<PointPerception>("points");
300+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
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: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -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_group<PointPerception>("points");
255+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
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 & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -566,13 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state)
566566
void
567567
AMCLLocalizer::correct(NavState & nav_state)
568568
{
569-
if (!nav_state.has_group("points")) {
570-
RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions");
569+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
570+
if (perceptions.empty()) {
571+
RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
571572
return;
572573
}
573574

574-
const auto & perceptions = nav_state.get_group<PointPerception>("points");
575-
576575
if (!nav_state.has("map.static")) {
577576
RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map");
578577
return;

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

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -142,11 +142,9 @@ void FusionLocalizer::update_rt(NavState & nav_state)
142142
if (!first_pose_received_) {
143143
RCLCPP_INFO(get_node()->get_logger(),
144144
"First valid GPS fix received. Initializing filter state.");
145-
if(nav_state.has("imu")) {
146-
auto imu_data = nav_state.get<IMUPerceptions>(std::string("imu"));
147-
if (!imu_data.empty()) {
148-
pose->pose.pose.orientation = imu_data[0]->data.orientation;
149-
}
145+
if (nav_state.has("imu")) {
146+
auto imu_data = nav_state.get<IMUPerception>(std::string("imu"));
147+
pose->pose.pose.orientation = imu_data.data.orientation;
150148
}
151149
ukf_global_->setPoseCallback(pose);
152150
first_pose_received_ = true;

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

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -496,9 +496,8 @@ void AMCLLocalizer::update_odom_from_tf()
496496
std::optional<tf2::Quaternion> get_latest_imu_quat(const NavState & nav_state)
497497
{
498498
if (!nav_state.has("imu")) {return std::nullopt;}
499-
const auto & imus = nav_state.get<IMUPerceptions>("imu");
500-
if (imus.empty() || !imus.back()) {return std::nullopt;}
501-
const auto & imu_msg = imus.back()->data;
499+
const auto & imu = nav_state.get<IMUPerception>("imu");
500+
const auto & imu_msg = imu.data;
502501
tf2::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y,
503502
imu_msg.orientation.z, imu_msg.orientation.w);
504503
if (q.length2() < 1e-12) {return std::nullopt;}
@@ -689,11 +688,11 @@ static ScoreAgg score_particle_sensor_cloud(
689688
// ---------- correct() ----------
690689
void AMCLLocalizer::correct(NavState & nav_state)
691690
{
692-
if (!nav_state.has_group("points")) {
693-
RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet");
691+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
692+
if (perceptions.empty()) {
693+
RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
694694
return;
695695
}
696-
const auto & perceptions = nav_state.get_group<PointPerception>("points");
697696

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

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

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -368,13 +368,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state)
368368

369369
void AMCLLocalizer::correct(NavState & nav_state)
370370
{
371-
if (!nav_state.has_group("points")) {
372-
RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions");
371+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
372+
if (perceptions.empty()) {
373+
RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
373374
return;
374375
}
375376

376-
const auto & perceptions = nav_state.get_group<PointPerception>("points");
377-
378377
if (!nav_state.has("map.static")) {
379378
RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map");
380379
return;

‎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
@@ -41,12 +41,12 @@ ObstacleFilter::on_initialize()
4141
void
4242
ObstacleFilter::update(NavState & nav_state)
4343
{
44-
if (!nav_state.has_group("points")) {
44+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
45+
if (perceptions.empty()) {
46+
RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
4547
return;
4648
}
4749

48-
const auto & perceptions = nav_state.get_group<PointPerception>("points");
49-
5050
auto dynamic_map_ptr = nav_state.get_ptr<Costmap2D>("map.dynamic.filtered");
5151
Costmap2D & dynamic_map = *dynamic_map_ptr;
5252
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();

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

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,9 +44,13 @@ ObstacleFilter::on_initialize()
4444
void ObstacleFilter::update(::easynav::NavState & nav_state)
4545
{
4646
if (!nav_state.has("map.navmap")) {return;}
47-
if (!nav_state.has_group("points")) {return;}
4847

49-
const auto & perceptions = nav_state.get_group<PointPerception>("points");
48+
const auto & perceptions = nav_state.get_no_group<PointPerception>();
49+
if (perceptions.empty()) {
50+
RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
51+
return;
52+
}
53+
5054
navmap_ = nav_state.get<::navmap::NavMap>("map.navmap");
5155
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
5256

0 commit comments

Comments
 (0)