From 2b36c5bc5238a6b842645a41fddcf01d6e155b7b Mon Sep 17 00:00:00 2001
From: Michael Jeronimo <michael.jeronimo@intel.com>
Date: Wed, 22 May 2019 10:59:32 -0700
Subject: [PATCH] Fix build issues after rebasing
---
nav2_amcl/include/nav2_amcl/amcl_node.hpp | 31 ++++++++++++++++
nav2_bringup/launch/nav2_params.yaml | 35 ++++++++++---------
.../src/navigate_to_pose_behavior_tree.cpp | 2 --
.../dwb_controller/src/dwb_controller.cpp | 7 ++--
.../dwb_controller/src/main.cpp | 2 +-
.../dwb_core/src/dwb_local_planner.cpp | 6 ++--
.../motion_primitive.hpp | 3 +-
nav2_rviz_plugins/src/navigation_dialog.cpp | 3 +-
.../nav2_tasks/goal_reached_condition.hpp | 9 +++--
nav2_util/src/motion_model/CMakeLists.txt | 1 +
nav2_util/test/CMakeLists.txt | 2 +-
11 files changed, 70 insertions(+), 31 deletions(-)
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 12de818a..8a4b3487 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -159,6 +159,37 @@ protected:
void checkLaserReceived();
std::chrono::seconds laser_check_interval_; // TODO(mjeronimo): not initialized
+ // Pose hypothesis
+ typedef struct
+ {
+ double weight; // Total weight (weights sum to 1)
+ pf_vector_t pf_pose_mean; // Mean of pose esimate
+ pf_matrix_t pf_pose_cov; // Covariance of pose estimate
+ } amcl_hyp_t;
+
+ bool addNewScanner(
+ int & laser_index,
+ const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+ const std::string & laser_scan_frame_id,
+ geometry_msgs::msg::PoseStamped & laser_pose);
+ bool shouldUpdateFilter(const pf_vector_t pose, pf_vector_t & delta);
+ bool updateFilter(
+ const int & laser_index,
+ const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+ const pf_vector_t & pose);
+ void publishParticleCloud(const pf_sample_set_t * set);
+ bool getMaxWeightHyp(
+ std::vector<amcl_hyp_t> & hyps, amcl_hyp_t & max_weight_hyps,
+ int & max_weight_hyp);
+ void publishAmclPose(
+ const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+ const std::vector<amcl_hyp_t> & hyps, const int & max_weight_hyp);
+ void calculateMaptoOdomTransform(
+ const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+ const std::vector<amcl_hyp_t> & hyps,
+ const int & max_weight_hyp);
+ void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration);
+
// Node parameters (initialized via initParameters)
void initParameters();
double alpha1_;
diff --git a/nav2_bringup/launch/nav2_params.yaml b/nav2_bringup/launch/nav2_params.yaml
index bdd966fb..e3d53837 100644
--- a/nav2_bringup/launch/nav2_params.yaml
+++ b/nav2_bringup/launch/nav2_params.yaml
@@ -80,7 +80,8 @@ dwb_controller:
sim_time: 1.7
linear_granularity: 0.05
xy_goal_tolerance: 0.25
- critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+ # critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+ critics: ["RotateToGoal", "Oscillation", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
GoalAlign.scale: 24.0
@@ -94,7 +95,7 @@ local_costmap:
robot_radius: 0.17
inflation_layer.cost_scaling_factor: 3.0
obstacle_layer:
- enabled: True
+ enabled: False
always_send_full_costmap: True
observation_sources: scan
scan:
@@ -103,12 +104,22 @@ local_costmap:
clearing: True
marking: True
+local_costmap:
+ local_costmap_client:
+ ros__parameters:
+ use_sim_time: True
+
+local_costmap:
+ local_costmap_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
global_costmap:
global_costmap:
ros__parameters:
robot_radius: 0.17
obstacle_layer:
- enabled: True
+ enabled: False
always_send_full_costmap: True
observation_sources: scan
scan:
@@ -117,13 +128,13 @@ global_costmap:
clearing: True
marking: True
-local_costmap:
- local_costmap_client:
+global_costmap:
+ global_costmap_client:
ros__parameters:
use_sim_time: True
-local_costmap:
- local_costmap_rclcpp_node:
+global_costmap:
+ global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
@@ -161,13 +172,3 @@ robot_state_publisher:
world_model:
ros__parameters:
use_sim_time: True
-
-global_costmap:
- global_costmap_client:
- ros__parameters:
- use_sim_time: True
-
-global_costmap:
- global_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: True
diff --git a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
index 00637b3f..e25fc0c0 100644
--- a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
+++ b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
@@ -59,7 +59,6 @@ NavigateToPoseBehaviorTree::NavigateToPoseBehaviorTree()
factory_.registerSimpleAction("clearEntirelyCostmapServiceRequest",
std::bind(&NavigateToPoseBehaviorTree::clearEntirelyCostmapServiceRequest, this,
std::placeholders::_1));
-}
global_localization_client_ =
std::make_unique<nav2_util::GlobalLocalizationServiceClient>("bt_navigator");
@@ -95,7 +94,6 @@ BT::NodeStatus NavigateToPoseBehaviorTree::clearEntirelyCostmapServiceRequest(
auto result = clear_entirely_costmap.invoke(request, std::chrono::seconds(3));
return BT::NodeStatus::SUCCESS;
} catch (std::runtime_error & e) {
- RCLCPP_WARN(node_->get_logger(), e.what());
return BT::NodeStatus::FAILURE;
}
}
diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
index 84d609a8..f2749c2c 100644
--- a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
@@ -25,7 +25,7 @@
using namespace std::chrono_literals;
-namespace nav2_dwb_controller
+namespace dwb_controller
{
DwbController::DwbController()
@@ -142,7 +142,8 @@ DwbController::followPath(const std::shared_ptr<GoalHandle> goal_handle)
RCLCPP_DEBUG(get_logger(), "Providing path to the local planner");
planner_->setPlan(path);
- ProgressChecker progress_checker(std::shared_ptr<rclcpp::Node>(this, [](auto) {}));
+ ProgressChecker progress_checker(rclcpp_node_);
+
rclcpp::Rate loop_rate(10);
while (rclcpp::ok()) {
nav_2d_msgs::msg::Pose2DStamped pose2d;
@@ -227,4 +228,4 @@ bool DwbController::getRobotPose(nav_2d_msgs::msg::Pose2DStamped & pose2d)
return true;
}
-} // namespace nav2_dwb_controller
+} // namespace dwb_controller
diff --git a/nav2_dwb_controller/dwb_controller/src/main.cpp b/nav2_dwb_controller/dwb_controller/src/main.cpp
index d50759ef..a48151fe 100644
--- a/nav2_dwb_controller/dwb_controller/src/main.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/main.cpp
@@ -20,7 +20,7 @@
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
- auto node = std::make_shared<nav2_dwb_controller::DwbController>();
+ auto node = std::make_shared<dwb_controller::DwbController>();
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();
diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index f825d013..db03abfc 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -91,8 +91,8 @@ DWBLocalPlanner::on_configure(const rclcpp_lifecycle::State & state)
traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name);
goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name);
- traj_generator_->initialize(nh_);
- goal_checker_->initialize(nh_);
+ traj_generator_->initialize(node_);
+ goal_checker_->initialize(node_);
loadCritics();
@@ -538,7 +538,7 @@ DWBLocalPlanner::transformGlobalPlan(
// process it on the next iteration.
if (prune_plan_) {
global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin);
- pub_.publishGlobalPlan(global_plan_);
+ pub_->publishGlobalPlan(global_plan_);
}
if (transformed_plan.poses.size() == 0) {
diff --git a/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp b/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp
index 3a84a9d4..00db066e 100644
--- a/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp
+++ b/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp
@@ -145,11 +145,12 @@ protected:
goal_handle->abort(result);
return;
+ case Status::RUNNING:
default:
+ loop_rate.sleep();
break;
}
- loop_rate.sleep();
}
}
};
diff --git a/nav2_rviz_plugins/src/navigation_dialog.cpp b/nav2_rviz_plugins/src/navigation_dialog.cpp
index 3d024ecc..1bcde8a5 100644
--- a/nav2_rviz_plugins/src/navigation_dialog.cpp
+++ b/nav2_rviz_plugins/src/navigation_dialog.cpp
@@ -65,7 +65,8 @@ void
NavigationDialog::timerEvent(QTimerEvent * event)
{
if (event->timerId() == timer_.timerId()) {
- auto future_result = action_client_->async_get_result(goal_handle_);
+ auto future_result = goal_handle_->async_result();
+
if (rclcpp::spin_until_future_complete(client_node_, future_result, 1ms) ==
rclcpp::executor::FutureReturnCode::TIMEOUT)
{
diff --git a/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp b/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp
index fa549927..e56b59ca 100644
--- a/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp
+++ b/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp
@@ -17,11 +17,12 @@
#include <string>
#include <memory>
+
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp/condition_node.h"
#include "nav2_robot/robot.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "nav2_tasks/compute_path_to_pose_task.hpp"
+//#include "nav2_tasks/compute_path_to_pose_task.hpp"
namespace nav2_tasks
{
@@ -56,13 +57,14 @@ public:
{
node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
- robot_ = std::make_unique<nav2_robot::Robot>(node_);
+ //robot_ = std::make_unique<nav2_robot::Robot>(node_);
initialized_ = true;
}
bool
goalReached()
{
+#if 0
auto current_pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
if (!robot_->getCurrentPose(current_pose)) {
@@ -79,6 +81,9 @@ public:
} else {
return false;
}
+#else
+ return false;
+#endif
}
private:
diff --git a/nav2_util/src/motion_model/CMakeLists.txt b/nav2_util/src/motion_model/CMakeLists.txt
index 1a14ea20..aac83917 100644
--- a/nav2_util/src/motion_model/CMakeLists.txt
+++ b/nav2_util/src/motion_model/CMakeLists.txt
@@ -1,5 +1,6 @@
add_library(motions_lib SHARED
omni_motion_model.cpp
differential_motion_model.cpp
+ motion_model.cpp
)
target_link_libraries(motions_lib pf_lib)
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index c608ad3c..74ac53d8 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -18,5 +18,5 @@ ament_target_dependencies(test_lifecycle_utils rclcpp_lifecycle)
target_link_libraries(test_lifecycle_utils ${library_name})
ament_add_gtest(test_actions test_actions.cpp)
-ament_target_dependencies(test_actions rclcpp_action)
+ament_target_dependencies(test_actions rclcpp_action test_msgs)
target_link_libraries(test_actions ${library_name})
--
GitLab