From 5f33ac55ee7bfa6f6e1cc294e500a720245feb69 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Tue, 1 Dec 2020 12:48:37 +0800 Subject: [PATCH 01/70] Add nav2_gps_waypoint_follower --- nav2_gps_waypoint_follower/CMakeLists.txt | 64 ++++ nav2_gps_waypoint_follower/README.md | 26 ++ .../gps_waypoint_follower.hpp | 159 +++++++++ nav2_gps_waypoint_follower/package.xml | 33 ++ .../src/gps_waypoint_follower.cpp | 301 ++++++++++++++++++ nav2_msgs/CMakeLists.txt | 4 +- nav2_msgs/action/FollowGPSWaypoints.action | 8 + nav2_msgs/package.xml | 1 + 8 files changed, 595 insertions(+), 1 deletion(-) create mode 100644 nav2_gps_waypoint_follower/CMakeLists.txt create mode 100644 nav2_gps_waypoint_follower/README.md create mode 100644 nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp create mode 100644 nav2_gps_waypoint_follower/package.xml create mode 100644 nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp create mode 100644 nav2_msgs/action/FollowGPSWaypoints.action diff --git a/nav2_gps_waypoint_follower/CMakeLists.txt b/nav2_gps_waypoint_follower/CMakeLists.txt new file mode 100644 index 0000000000..1b7bb2329d --- /dev/null +++ b/nav2_gps_waypoint_follower/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_gps_waypoint_follower) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_lifecycle_manager REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(robot_localization REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +nav2_package() + +include_directories( + include +) + +set(executable_name gps_waypoint_follower) + +add_executable(${executable_name} + src/gps_waypoint_follower.cpp +) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + nav2_util + nav2_lifecycle_manager + nav_msgs + nav2_msgs + nav2_core + tf2_ros + robot_localization +) + +ament_target_dependencies(${executable_name} + ${dependencies} +) + +target_link_libraries(${executable_name}) + +install(TARGETS ${executable_name} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) + +ament_package() diff --git a/nav2_gps_waypoint_follower/README.md b/nav2_gps_waypoint_follower/README.md new file mode 100644 index 0000000000..ed9bbc99a7 --- /dev/null +++ b/nav2_gps_waypoint_follower/README.md @@ -0,0 +1,26 @@ +# Nav2 GPS Waypoint Follower + +This package is analogous to `nav2_waypoint_follower`, `nav2_gps_waypoint_follower` provides an action server interface that accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower`. The action msg has following properties; + +```yaml +#goal definition +sensor_msgs/NavSatFix[] waypoints +--- +#result definition +int32[] missed_waypoints +--- +#feedback +uint32 current_waypoint +``` + +A use case can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. +For instance; + +```cpp +using ClientT = nav2_msgs::action::FollowGPSWaypoints; +rclcpp_action::Client::SharedPtr gps_waypoint_follower_action_client_; +gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "FollowGPSWaypoints"); + +``` + +All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file diff --git a/nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp b/nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp new file mode 100644 index 0000000000..edaea08005 --- /dev/null +++ b/nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp @@ -0,0 +1,159 @@ +// Copyright (c) 2020 Fetullah Atas +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_ +#define NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_ + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "geometry_msgs/msg/point32.hpp" +#include "nav2_msgs/action/follow_gps_waypoints.hpp" +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" +#include "nav2_msgs/action/follow_waypoints.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/simple_action_server.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "robot_localization/srv/to_ll.hpp" +#include "robot_localization/srv/from_ll.hpp" + +/** + * @brief namespace for way point following, points are from a yaml file + * + */ +namespace nav2_gps_waypoint_follower +{ +enum class ActionStatus +{ + UNKNOWN = 0, + PROCESSING = 1, + FAILED = 2, + SUCCEEDED = 3 +}; +/** + * @brief A ros lifecyle node that drives robot through gievn way points from YAML file + * + */ +class GPSWaypointFollower : public nav2_util::LifecycleNode +{ +public: + // Shorten the types + using ActionT = nav2_msgs::action::FollowGPSWaypoints; + using ClientT = nav2_msgs::action::FollowWaypoints; + using ActionServer = nav2_util::SimpleActionServer; + using ActionClient = rclcpp_action::Client; + using WaypointFollowerGoalHandle = + rclcpp_action::ClientGoalHandle; + + /** + * @brief Construct a new Way Point Folllower Demo object + * + */ + GPSWaypointFollower(); + + /** + * @brief Destroy the Way Point Folllower Demo object + * + */ + ~GPSWaypointFollower(); + +protected: + /** + * @brief Configures member variables + * + * Initializes action server for "FollowWaypoints" + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief Activates action server + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Deactivates action server + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Resets member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when in shutdown state + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Action client result callback + * @param result Result of action server updated asynchronously + */ + void resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + + /** + * @brief Action client goal response callback + * @param goal Response of action server updated asynchronously + */ + void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + + /** + * @brief send robot through each of the pose in poses vector + * + * @param poses + */ + void followGPSWaypoints(); + + /** + * @brief + * + */ + static std::vector convertGPSWaypointstoPosesinMap( + const + std::vector & gps_waypoints, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, + const rclcpp::Client::SharedPtr & fromll_client); + + // dedicated node of client(FollowWaypoints) + rclcpp::Node::SharedPtr client_node_; + // FollowGPSWaypoints action server + std::unique_ptr action_server_; + // client to call server from robot_localization to do UTM -> Map conversion + rclcpp::Client::SharedPtr from_ll_to_map_client_; + // client to connect waypoint follower service(FollowWaypoints) + rclcpp_action::Client::SharedPtr waypoint_follower_action_client_; + // global var to get information about current goal state + ActionStatus current_goal_status_; + // stores the waypoints in a vector with additional info such as + // "int32[] missed_waypoints" and "uint32 + // current_waypoint" + ClientT::Goal waypoint_follower_goal_; + // goal handler to query state of goal + WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_; + + rclcpp::CallbackGroup::SharedPtr callback_group_; +}; +} // namespace nav2_gps_waypoint_follower + +#endif // NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_ diff --git a/nav2_gps_waypoint_follower/package.xml b/nav2_gps_waypoint_follower/package.xml new file mode 100644 index 0000000000..cff58bfe8b --- /dev/null +++ b/nav2_gps_waypoint_follower/package.xml @@ -0,0 +1,33 @@ + + + + nav2_gps_waypoint_follower + 0.0.0 + An action server interface for GPS waypoint following, converts GPS points to map frame with service + from `robot_localization` and navigates through the points + with FollowWaypoints action server from `nav2_waypoint_follower` + Fetullah Atas + Apache-2.0 + ament_cmake + tf2_ros + nav2_common + rclcpp + rclcpp_action + rclcpp_lifecycle + nav_msgs + sensor_msgs + nav2_msgs + nav2_util + nav2_core + geometry_msgs + nav2_lifecycle_manager + std_msgs + tf2_geometry_msgs + visualization_msgs + robot_localization + ament_lint_common + ament_lint_auto + + ament_cmake + + \ No newline at end of file diff --git a/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp b/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp new file mode 100644 index 0000000000..c7efeb22ad --- /dev/null +++ b/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp @@ -0,0 +1,301 @@ +// Copyright (c) 2020 Fetullah Atas +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include "nav2_gps_waypoint_follower/gps_waypoint_follower.hpp" + +namespace nav2_gps_waypoint_follower +{ +GPSWaypointFollower::GPSWaypointFollower() +: nav2_util::LifecycleNode("GPSWaypointFollower", "", false) +{ + RCLCPP_INFO(get_logger(), "Creating"); +} + +GPSWaypointFollower::~GPSWaypointFollower() +{ + RCLCPP_INFO(get_logger(), "Destroying"); +} + +nav2_util::CallbackReturn +GPSWaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + std::vector new_args = rclcpp::NodeOptions().arguments(); + new_args.push_back("--ros-args"); + new_args.push_back("-r"); + new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); + new_args.push_back("--"); + client_node_ = std::make_shared( + "_", "", rclcpp::NodeOptions().arguments(new_args)); + + waypoint_follower_goal_ = ClientT::Goal(); + waypoint_follower_action_client_ = rclcpp_action::create_client( + client_node_, "FollowWaypoints"); + + callback_group_ = this->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); + from_ll_to_map_client_ = + this->create_client( + "/fromLL", + rmw_qos_profile_services_default, + callback_group_); + + action_server_ = std::make_unique( + get_node_base_interface(), + get_node_clock_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + "FollowGPSWaypoints", std::bind(&GPSWaypointFollower::followGPSWaypoints, this)); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +GPSWaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + action_server_->activate(); + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +GPSWaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + action_server_->deactivate(); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +GPSWaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + action_server_.reset(); + waypoint_follower_action_client_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +GPSWaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +void GPSWaypointFollower::followGPSWaypoints() +{ + auto goal = action_server_->get_current_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + if (!action_server_ || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); + return; + } + + RCLCPP_INFO( + get_logger(), "Received follow GPS waypoint request with %i waypoints.", + static_cast(goal->waypoints.size())); + + std::vector poses = convertGPSWaypointstoPosesinMap( + goal->waypoints, shared_from_this(), from_ll_to_map_client_); + + auto is_action_server_ready = + waypoint_follower_action_client_->wait_for_action_server(std::chrono::seconds(5)); + if (!is_action_server_ready) { + RCLCPP_ERROR( + this->get_logger(), "FollowWaypoints action server is not available." + "make an instance of waypoint_follower is up and running"); + return; + } + waypoint_follower_goal_.poses = poses; + + RCLCPP_INFO( + this->get_logger(), + "Sending a path of %zu waypoints:", waypoint_follower_goal_.poses.size()); + for (auto waypoint : waypoint_follower_goal_.poses) { + RCLCPP_DEBUG( + this->get_logger(), + "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); + } + + auto goal_options = + rclcpp_action::Client::SendGoalOptions(); + goal_options.result_callback = std::bind( + &GPSWaypointFollower::resultCallback, this, + std::placeholders::_1); + goal_options.goal_response_callback = std::bind( + &GPSWaypointFollower::goalResponseCallback, this, + std::placeholders::_1); + + auto future_goal_handle = waypoint_follower_action_client_->async_send_goal( + waypoint_follower_goal_, goal_options); + if (rclcpp::spin_until_future_complete( + this->get_node_base_interface(), + future_goal_handle) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(get_logger(), "Send goal call failed"); + return; + } + // Get the goal handle and save so that we can check + // on completion in the timer callback + waypoint_follower_goal_handle_ = future_goal_handle.get(); + if (!waypoint_follower_goal_handle_) { + RCLCPP_ERROR(get_logger(), "Goal was rejected by server"); + return; + } +} + +std::vector +GPSWaypointFollower::convertGPSWaypointstoPosesinMap( + const + std::vector & gps_waypoints, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, + const rclcpp::Client::SharedPtr & fromll_client) +{ + RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); + if (!parent_node) { + throw std::runtime_error{ + "Failed to lock node while trying to convert GPS waypoints to Map frame"}; + } + std::vector poses_in_map_frame_vector; + int index_of_gps_waypoints = 0; + for (auto && curr_gps_waypoint : gps_waypoints) { + auto fromLLRequest = std::make_shared(); + fromLLRequest->ll_point.latitude = curr_gps_waypoint.latitude; + fromLLRequest->ll_point.longitude = curr_gps_waypoint.longitude; + fromLLRequest->ll_point.altitude = curr_gps_waypoint.altitude; + if (!fromll_client->wait_for_service((std::chrono::seconds(10)))) { + RCLCPP_ERROR( + parent_node->get_logger(), + "fromLL service from robot_localization is not available" + "cannot convert GPS waypoints to Map frame poses," + "Make sure you have run navsat_transform_node of robot_localization"); + return std::vector(); + } + RCLCPP_INFO( + parent_node->get_logger(), "Processing for %i'th Waypoint..", + index_of_gps_waypoints); + auto inner_client_callback = + [&, parent_node](rclcpp::Client::SharedFuture inner_future) + { + auto result = inner_future.get(); + }; + auto inner_future_result = fromll_client->async_send_request( + fromLLRequest, + inner_client_callback); + // this poses are assumed to be on global frame (map) + geometry_msgs::msg::PoseStamped curr_waypoint_in_map_frame; + curr_waypoint_in_map_frame.header.frame_id = "map"; + curr_waypoint_in_map_frame.header.stamp = rclcpp::Clock().now(); + curr_waypoint_in_map_frame.pose.position.x = inner_future_result.get()->map_point.x; + curr_waypoint_in_map_frame.pose.position.y = inner_future_result.get()->map_point.y; + curr_waypoint_in_map_frame.pose.position.z = inner_future_result.get()->map_point.z; + + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, 0); + geometry_msgs::msg::Quaternion quat_msg; + tf2::convert(quat_msg, quat_tf); + curr_waypoint_in_map_frame.pose.orientation = quat_msg; + + RCLCPP_INFO( + parent_node->get_logger(), + "%i th Waypoint Long, Lat: %.8f , %.8f converted to " + "Map Point X,Y: %.8f , %.8f", index_of_gps_waypoints, fromLLRequest->ll_point.longitude, + fromLLRequest->ll_point.latitude, curr_waypoint_in_map_frame.pose.position.x, + curr_waypoint_in_map_frame.pose.position.y); + index_of_gps_waypoints++; + poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); + } + RCLCPP_INFO( + parent_node->get_logger(), + "Converted all %i GPS waypoint to Map frame", poses_in_map_frame_vector.size()); + return poses_in_map_frame_vector; +} + +void GPSWaypointFollower::resultCallback( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + current_goal_status_ = ActionStatus::SUCCEEDED; + return; + case rclcpp_action::ResultCode::ABORTED: + current_goal_status_ = ActionStatus::FAILED; + return; + case rclcpp_action::ResultCode::CANCELED: + current_goal_status_ = ActionStatus::FAILED; + return; + default: + current_goal_status_ = ActionStatus::UNKNOWN; + return; + } + + RCLCPP_INFO(this->get_logger(), "Result received"); + for (auto number : result.result->missed_waypoints) { + RCLCPP_INFO( + this->get_logger(), + "Missed" + "%d points from given Yaml waypoints", number); + } +} + +void GPSWaypointFollower::goalResponseCallback( + const + rclcpp_action::ClientGoalHandle::SharedPtr & goal) +{ + if (!goal) { + RCLCPP_ERROR( + get_logger(), + "navigate_to_pose action client failed to send goal to server."); + current_goal_status_ = ActionStatus::FAILED; + } +} +} // namespace nav2_gps_waypoint_follower + +/** + * @brief Entry point for Way Point following demo Node + * + * @param argc + * @param argv + * @return int + */ +int main(int argc, char const * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared + (); + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index d09cafa74c..c770c1364c 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs) find_package(action_msgs REQUIRED) nav2_package() @@ -36,7 +37,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/Spin.action" "action/DummyRecovery.action" "action/FollowWaypoints.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs + "action/FollowGPSWaypoints.action" + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs sensor_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action new file mode 100644 index 0000000000..d83a59170a --- /dev/null +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -0,0 +1,8 @@ +#goal definition +sensor_msgs/NavSatFix[] waypoints +--- +#result definition +int32[] missed_waypoints +--- +#feedback +uint32 current_waypoint diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 00265980c7..e4a908703c 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -19,6 +19,7 @@ geometry_msgs action_msgs nav_msgs + sensor_msgs rosidl_interface_packages From 277c1e1658bad1bfeec379c815eb120d512413e1 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 2 Dec 2020 10:32:15 +0800 Subject: [PATCH 02/70] use correct client node while calling it to spin --- nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp b/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp index c7efeb22ad..05e939cf60 100644 --- a/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp +++ b/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp @@ -157,7 +157,7 @@ void GPSWaypointFollower::followGPSWaypoints() auto future_goal_handle = waypoint_follower_action_client_->async_send_goal( waypoint_follower_goal_, goal_options); if (rclcpp::spin_until_future_complete( - this->get_node_base_interface(), + client_node_, future_goal_handle) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(get_logger(), "Send goal call failed"); From c6ee43704ee67f41872dfafe527f69fa8eca4d3c Mon Sep 17 00:00:00 2001 From: jediofgever Date: Fri, 4 Dec 2020 13:24:57 +0800 Subject: [PATCH 03/70] changed after 1'st review --- nav2_gps_waypoint_follower/CMakeLists.txt | 64 ---- nav2_gps_waypoint_follower/README.md | 26 -- .../gps_waypoint_follower.hpp | 159 --------- nav2_gps_waypoint_follower/package.xml | 33 -- .../src/gps_waypoint_follower.cpp | 301 ------------------ nav2_waypoint_follower/CMakeLists.txt | 2 + nav2_waypoint_follower/README.md | 26 ++ .../waypoint_follower.hpp | 61 +++- nav2_waypoint_follower/package.xml | 3 +- .../src/waypoint_follower.cpp | 207 +++++++++++- 10 files changed, 290 insertions(+), 592 deletions(-) delete mode 100644 nav2_gps_waypoint_follower/CMakeLists.txt delete mode 100644 nav2_gps_waypoint_follower/README.md delete mode 100644 nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp delete mode 100644 nav2_gps_waypoint_follower/package.xml delete mode 100644 nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp diff --git a/nav2_gps_waypoint_follower/CMakeLists.txt b/nav2_gps_waypoint_follower/CMakeLists.txt deleted file mode 100644 index 1b7bb2329d..0000000000 --- a/nav2_gps_waypoint_follower/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(nav2_gps_waypoint_follower) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(nav2_common REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav2_lifecycle_manager REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(robot_localization REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -nav2_package() - -include_directories( - include -) - -set(executable_name gps_waypoint_follower) - -add_executable(${executable_name} - src/gps_waypoint_follower.cpp -) - -set(dependencies - rclcpp - rclcpp_action - rclcpp_lifecycle - nav2_util - nav2_lifecycle_manager - nav_msgs - nav2_msgs - nav2_core - tf2_ros - robot_localization -) - -ament_target_dependencies(${executable_name} - ${dependencies} -) - -target_link_libraries(${executable_name}) - -install(TARGETS ${executable_name} - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include/ -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_export_include_directories(include) - -ament_package() diff --git a/nav2_gps_waypoint_follower/README.md b/nav2_gps_waypoint_follower/README.md deleted file mode 100644 index ed9bbc99a7..0000000000 --- a/nav2_gps_waypoint_follower/README.md +++ /dev/null @@ -1,26 +0,0 @@ -# Nav2 GPS Waypoint Follower - -This package is analogous to `nav2_waypoint_follower`, `nav2_gps_waypoint_follower` provides an action server interface that accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower`. The action msg has following properties; - -```yaml -#goal definition -sensor_msgs/NavSatFix[] waypoints ---- -#result definition -int32[] missed_waypoints ---- -#feedback -uint32 current_waypoint -``` - -A use case can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. -For instance; - -```cpp -using ClientT = nav2_msgs::action::FollowGPSWaypoints; -rclcpp_action::Client::SharedPtr gps_waypoint_follower_action_client_; -gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "FollowGPSWaypoints"); - -``` - -All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file diff --git a/nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp b/nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp deleted file mode 100644 index edaea08005..0000000000 --- a/nav2_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp +++ /dev/null @@ -1,159 +0,0 @@ -// Copyright (c) 2020 Fetullah Atas -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_ -#define NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_ - -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "geometry_msgs/msg/point32.hpp" -#include "nav2_msgs/action/follow_gps_waypoints.hpp" -#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" -#include "nav2_msgs/action/follow_waypoints.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" -#include "nav2_util/simple_action_server.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "robot_localization/srv/to_ll.hpp" -#include "robot_localization/srv/from_ll.hpp" - -/** - * @brief namespace for way point following, points are from a yaml file - * - */ -namespace nav2_gps_waypoint_follower -{ -enum class ActionStatus -{ - UNKNOWN = 0, - PROCESSING = 1, - FAILED = 2, - SUCCEEDED = 3 -}; -/** - * @brief A ros lifecyle node that drives robot through gievn way points from YAML file - * - */ -class GPSWaypointFollower : public nav2_util::LifecycleNode -{ -public: - // Shorten the types - using ActionT = nav2_msgs::action::FollowGPSWaypoints; - using ClientT = nav2_msgs::action::FollowWaypoints; - using ActionServer = nav2_util::SimpleActionServer; - using ActionClient = rclcpp_action::Client; - using WaypointFollowerGoalHandle = - rclcpp_action::ClientGoalHandle; - - /** - * @brief Construct a new Way Point Folllower Demo object - * - */ - GPSWaypointFollower(); - - /** - * @brief Destroy the Way Point Folllower Demo object - * - */ - ~GPSWaypointFollower(); - -protected: - /** - * @brief Configures member variables - * - * Initializes action server for "FollowWaypoints" - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; - /** - * @brief Activates action server - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Deactivates action server - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Resets member variables - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in shutdown state - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - - /** - * @brief Action client result callback - * @param result Result of action server updated asynchronously - */ - void resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); - - /** - * @brief Action client goal response callback - * @param goal Response of action server updated asynchronously - */ - void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); - - /** - * @brief send robot through each of the pose in poses vector - * - * @param poses - */ - void followGPSWaypoints(); - - /** - * @brief - * - */ - static std::vector convertGPSWaypointstoPosesinMap( - const - std::vector & gps_waypoints, - const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - const rclcpp::Client::SharedPtr & fromll_client); - - // dedicated node of client(FollowWaypoints) - rclcpp::Node::SharedPtr client_node_; - // FollowGPSWaypoints action server - std::unique_ptr action_server_; - // client to call server from robot_localization to do UTM -> Map conversion - rclcpp::Client::SharedPtr from_ll_to_map_client_; - // client to connect waypoint follower service(FollowWaypoints) - rclcpp_action::Client::SharedPtr waypoint_follower_action_client_; - // global var to get information about current goal state - ActionStatus current_goal_status_; - // stores the waypoints in a vector with additional info such as - // "int32[] missed_waypoints" and "uint32 - // current_waypoint" - ClientT::Goal waypoint_follower_goal_; - // goal handler to query state of goal - WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_; - - rclcpp::CallbackGroup::SharedPtr callback_group_; -}; -} // namespace nav2_gps_waypoint_follower - -#endif // NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_ diff --git a/nav2_gps_waypoint_follower/package.xml b/nav2_gps_waypoint_follower/package.xml deleted file mode 100644 index cff58bfe8b..0000000000 --- a/nav2_gps_waypoint_follower/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - nav2_gps_waypoint_follower - 0.0.0 - An action server interface for GPS waypoint following, converts GPS points to map frame with service - from `robot_localization` and navigates through the points - with FollowWaypoints action server from `nav2_waypoint_follower` - Fetullah Atas - Apache-2.0 - ament_cmake - tf2_ros - nav2_common - rclcpp - rclcpp_action - rclcpp_lifecycle - nav_msgs - sensor_msgs - nav2_msgs - nav2_util - nav2_core - geometry_msgs - nav2_lifecycle_manager - std_msgs - tf2_geometry_msgs - visualization_msgs - robot_localization - ament_lint_common - ament_lint_auto - - ament_cmake - - \ No newline at end of file diff --git a/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp b/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp deleted file mode 100644 index 05e939cf60..0000000000 --- a/nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp +++ /dev/null @@ -1,301 +0,0 @@ -// Copyright (c) 2020 Fetullah Atas -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include "nav2_gps_waypoint_follower/gps_waypoint_follower.hpp" - -namespace nav2_gps_waypoint_follower -{ -GPSWaypointFollower::GPSWaypointFollower() -: nav2_util::LifecycleNode("GPSWaypointFollower", "", false) -{ - RCLCPP_INFO(get_logger(), "Creating"); -} - -GPSWaypointFollower::~GPSWaypointFollower() -{ - RCLCPP_INFO(get_logger(), "Destroying"); -} - -nav2_util::CallbackReturn -GPSWaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Configuring"); - std::vector new_args = rclcpp::NodeOptions().arguments(); - new_args.push_back("--ros-args"); - new_args.push_back("-r"); - new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); - new_args.push_back("--"); - client_node_ = std::make_shared( - "_", "", rclcpp::NodeOptions().arguments(new_args)); - - waypoint_follower_goal_ = ClientT::Goal(); - waypoint_follower_action_client_ = rclcpp_action::create_client( - client_node_, "FollowWaypoints"); - - callback_group_ = this->create_callback_group( - rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); - from_ll_to_map_client_ = - this->create_client( - "/fromLL", - rmw_qos_profile_services_default, - callback_group_); - - action_server_ = std::make_unique( - get_node_base_interface(), - get_node_clock_interface(), - get_node_logging_interface(), - get_node_waitables_interface(), - "FollowGPSWaypoints", std::bind(&GPSWaypointFollower::followGPSWaypoints, this)); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -GPSWaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Activating"); - - action_server_->activate(); - - // create bond connection - createBond(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -GPSWaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Deactivating"); - - action_server_->deactivate(); - - // destroy bond connection - destroyBond(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -GPSWaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Cleaning up"); - - action_server_.reset(); - waypoint_follower_action_client_.reset(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -GPSWaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Shutting down"); - return nav2_util::CallbackReturn::SUCCESS; -} - -void GPSWaypointFollower::followGPSWaypoints() -{ - auto goal = action_server_->get_current_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - if (!action_server_ || !action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); - return; - } - - RCLCPP_INFO( - get_logger(), "Received follow GPS waypoint request with %i waypoints.", - static_cast(goal->waypoints.size())); - - std::vector poses = convertGPSWaypointstoPosesinMap( - goal->waypoints, shared_from_this(), from_ll_to_map_client_); - - auto is_action_server_ready = - waypoint_follower_action_client_->wait_for_action_server(std::chrono::seconds(5)); - if (!is_action_server_ready) { - RCLCPP_ERROR( - this->get_logger(), "FollowWaypoints action server is not available." - "make an instance of waypoint_follower is up and running"); - return; - } - waypoint_follower_goal_.poses = poses; - - RCLCPP_INFO( - this->get_logger(), - "Sending a path of %zu waypoints:", waypoint_follower_goal_.poses.size()); - for (auto waypoint : waypoint_follower_goal_.poses) { - RCLCPP_DEBUG( - this->get_logger(), - "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); - } - - auto goal_options = - rclcpp_action::Client::SendGoalOptions(); - goal_options.result_callback = std::bind( - &GPSWaypointFollower::resultCallback, this, - std::placeholders::_1); - goal_options.goal_response_callback = std::bind( - &GPSWaypointFollower::goalResponseCallback, this, - std::placeholders::_1); - - auto future_goal_handle = waypoint_follower_action_client_->async_send_goal( - waypoint_follower_goal_, goal_options); - if (rclcpp::spin_until_future_complete( - client_node_, - future_goal_handle) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(get_logger(), "Send goal call failed"); - return; - } - // Get the goal handle and save so that we can check - // on completion in the timer callback - waypoint_follower_goal_handle_ = future_goal_handle.get(); - if (!waypoint_follower_goal_handle_) { - RCLCPP_ERROR(get_logger(), "Goal was rejected by server"); - return; - } -} - -std::vector -GPSWaypointFollower::convertGPSWaypointstoPosesinMap( - const - std::vector & gps_waypoints, - const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - const rclcpp::Client::SharedPtr & fromll_client) -{ - RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); - if (!parent_node) { - throw std::runtime_error{ - "Failed to lock node while trying to convert GPS waypoints to Map frame"}; - } - std::vector poses_in_map_frame_vector; - int index_of_gps_waypoints = 0; - for (auto && curr_gps_waypoint : gps_waypoints) { - auto fromLLRequest = std::make_shared(); - fromLLRequest->ll_point.latitude = curr_gps_waypoint.latitude; - fromLLRequest->ll_point.longitude = curr_gps_waypoint.longitude; - fromLLRequest->ll_point.altitude = curr_gps_waypoint.altitude; - if (!fromll_client->wait_for_service((std::chrono::seconds(10)))) { - RCLCPP_ERROR( - parent_node->get_logger(), - "fromLL service from robot_localization is not available" - "cannot convert GPS waypoints to Map frame poses," - "Make sure you have run navsat_transform_node of robot_localization"); - return std::vector(); - } - RCLCPP_INFO( - parent_node->get_logger(), "Processing for %i'th Waypoint..", - index_of_gps_waypoints); - auto inner_client_callback = - [&, parent_node](rclcpp::Client::SharedFuture inner_future) - { - auto result = inner_future.get(); - }; - auto inner_future_result = fromll_client->async_send_request( - fromLLRequest, - inner_client_callback); - // this poses are assumed to be on global frame (map) - geometry_msgs::msg::PoseStamped curr_waypoint_in_map_frame; - curr_waypoint_in_map_frame.header.frame_id = "map"; - curr_waypoint_in_map_frame.header.stamp = rclcpp::Clock().now(); - curr_waypoint_in_map_frame.pose.position.x = inner_future_result.get()->map_point.x; - curr_waypoint_in_map_frame.pose.position.y = inner_future_result.get()->map_point.y; - curr_waypoint_in_map_frame.pose.position.z = inner_future_result.get()->map_point.z; - - tf2::Quaternion quat_tf; - quat_tf.setRPY(0, 0, 0); - geometry_msgs::msg::Quaternion quat_msg; - tf2::convert(quat_msg, quat_tf); - curr_waypoint_in_map_frame.pose.orientation = quat_msg; - - RCLCPP_INFO( - parent_node->get_logger(), - "%i th Waypoint Long, Lat: %.8f , %.8f converted to " - "Map Point X,Y: %.8f , %.8f", index_of_gps_waypoints, fromLLRequest->ll_point.longitude, - fromLLRequest->ll_point.latitude, curr_waypoint_in_map_frame.pose.position.x, - curr_waypoint_in_map_frame.pose.position.y); - index_of_gps_waypoints++; - poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); - } - RCLCPP_INFO( - parent_node->get_logger(), - "Converted all %i GPS waypoint to Map frame", poses_in_map_frame_vector.size()); - return poses_in_map_frame_vector; -} - -void GPSWaypointFollower::resultCallback( - const rclcpp_action::ClientGoalHandle - ::WrappedResult & result) -{ - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - current_goal_status_ = ActionStatus::SUCCEEDED; - return; - case rclcpp_action::ResultCode::ABORTED: - current_goal_status_ = ActionStatus::FAILED; - return; - case rclcpp_action::ResultCode::CANCELED: - current_goal_status_ = ActionStatus::FAILED; - return; - default: - current_goal_status_ = ActionStatus::UNKNOWN; - return; - } - - RCLCPP_INFO(this->get_logger(), "Result received"); - for (auto number : result.result->missed_waypoints) { - RCLCPP_INFO( - this->get_logger(), - "Missed" - "%d points from given Yaml waypoints", number); - } -} - -void GPSWaypointFollower::goalResponseCallback( - const - rclcpp_action::ClientGoalHandle::SharedPtr & goal) -{ - if (!goal) { - RCLCPP_ERROR( - get_logger(), - "navigate_to_pose action client failed to send goal to server."); - current_goal_status_ = ActionStatus::FAILED; - } -} -} // namespace nav2_gps_waypoint_follower - -/** - * @brief Entry point for Way Point following demo Node - * - * @param argc - * @param argv - * @return int - */ -int main(int argc, char const * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared - (); - executor.add_node(node->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - - return 0; -} diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 3253ab3439..7f2e2cab58 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(nav2_util REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_core REQUIRED) find_package(pluginlib REQUIRED) +find_package(robot_localization REQUIRED) nav2_package() @@ -53,6 +54,7 @@ set(dependencies image_transport cv_bridge OpenCV + robot_localization ) ament_target_dependencies(${executable_name} diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 5b8811c357..b5ad7baf05 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -24,3 +24,29 @@ In the first, the ``nav2_waypoint_follower`` is fully sufficient to create a pro In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy. Neither is better than the other, it highly depends on the tasks your robot(s) are completing, in what type of environment, and with what cloud resources available. Often this distinction is very clear for a given business case. + +## Nav2 GPS Waypoint Follower + +`nav2_waypoint_follower` provides an action server interface that accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself. The action for GPS waypoint following has properties as; + +```yaml +#goal definition +sensor_msgs/NavSatFix[] waypoints +--- +#result definition +int32[] missed_waypoints +--- +#feedback +uint32 current_waypoint +``` + +In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. +For instance; + +```cpp +using ClientT = nav2_msgs::action::FollowGPSWaypoints; +rclcpp_action::Client::SharedPtr gps_waypoint_follower_action_client_; +gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "FollowGPSWaypoints"); +``` + +All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 37b9633eba..75866e39a9 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -30,6 +30,10 @@ #include "nav2_core/waypoint_task_executor.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "nav2_msgs/action/follow_gps_waypoints.hpp" +#include "robot_localization/srv/from_ll.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" namespace nav2_waypoint_follower { @@ -55,6 +59,13 @@ class WaypointFollower : public nav2_util::LifecycleNode using ActionServer = nav2_util::SimpleActionServer; using ActionClient = rclcpp_action::Client; + // Shorten the types for GPS waypoint following + using ActionTGPS = nav2_msgs::action::FollowGPSWaypoints; + using ClientTGPS = nav2_msgs::action::FollowWaypoints; + using ActionServerGPS = nav2_util::SimpleActionServer; + using ActionClientGPS = rclcpp_action::Client; + using WaypointFollowerGoalHandle = + rclcpp_action::ClientGoalHandle; /** * @brief A constructor for nav2_waypoint_follower::WaypointFollower class */ @@ -115,16 +126,62 @@ class WaypointFollower : public nav2_util::LifecycleNode */ void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); - // Our action server + /** + * @brief send robot through each of GPS + * point , which are converted to map frame first then using a client to + * `FollowWaypoints` action. + * + * @param waypoints, acquired from action client + */ + void followGPSWaypoints(); + +/** + * @brief given some gps_waypoints, converts them to map frame using robot_localization's service `fromLL`. + * Constructs a vector of stamped poses in map frame and returns them. + * + * @param gps_waypoints + * @param parent_node + * @param fromll_client + * @return std::vector + */ + static std::vector convertGPSWaypointstoPosesinMap( + const + std::vector & gps_waypoints, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, + const rclcpp::Client::SharedPtr & fromll_client); + + /** + * @brief Action client result callback + * @param result Result of action server updated asynchronously + */ + void GPSResultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + + /** + * @brief Action client goal response callback + * @param goal Response of action server updated asynchronously + */ + void GPSGoalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + + // Our action server for waypoint following std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; - rclcpp::Node::SharedPtr client_node_; + rclcpp::Node::SharedPtr nav_to_pose_client_node_; std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; std::vector failed_ids_; + // Our action server for GPS waypoint following + std::unique_ptr gps_action_server_; + rclcpp::Client::SharedPtr from_ll_to_map_client_; + ActionClientGPS::SharedPtr waypoint_follower_action_client_; + rclcpp::Node::SharedPtr waypoint_follower_client_node_; + ActionStatus gps_current_goal_status_; + ClientTGPS::Goal waypoint_follower_goal_; + WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + // Task Execution At Waypoint Plugin pluginlib::ClassLoader waypoint_task_executor_loader_; diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index f4e28a6357..f16494af58 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -21,7 +21,8 @@ nav2_util nav2_core tf2_ros - + robot_localization + ament_lint_common ament_lint_auto diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index ff81e7666d..f4685dcd30 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -55,17 +55,17 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); - + // related to waypoint following std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); new_args.push_back("-r"); new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); new_args.push_back("--"); - client_node_ = std::make_shared( + nav_to_pose_client_node_ = std::make_shared( "_", "", rclcpp::NodeOptions().arguments(new_args)); nav_to_pose_client_ = rclcpp_action::create_client( - client_node_, "navigate_to_pose"); + nav_to_pose_client_node_, "navigate_to_pose"); action_server_ = std::make_unique( get_node_base_interface(), @@ -74,6 +74,27 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this)); + // related to GPS waypoint following + waypoint_follower_client_node_ = std::make_shared( + "_", "", rclcpp::NodeOptions().arguments(new_args)); + + waypoint_follower_action_client_ = rclcpp_action::create_client( + waypoint_follower_client_node_, "FollowWaypoints"); + + callback_group_ = this->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); + from_ll_to_map_client_ = + this->create_client( + "/fromLL", + rmw_qos_profile_services_default, + callback_group_); + + gps_action_server_ = std::make_unique( + get_node_base_interface(), + get_node_clock_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypoints, this)); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( this, @@ -99,6 +120,7 @@ WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); action_server_->activate(); + gps_action_server_->activate(); // create bond connection createBond(); @@ -112,6 +134,7 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); action_server_->deactivate(); + gps_action_server_->deactivate(); // destroy bond connection destroyBond(); @@ -126,6 +149,8 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_.reset(); nav_to_pose_client_.reset(); + gps_action_server_.reset(); + waypoint_follower_action_client_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -167,9 +192,9 @@ WaypointFollower::followWaypoints() // Check if asked to stop processing action if (action_server_->is_cancel_requested()) { auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); - rclcpp::spin_until_future_complete(client_node_, cancel_future); + rclcpp::spin_until_future_complete(nav_to_pose_client_node_, cancel_future); // for result callback processing - spin_some(client_node_); + spin_some(nav_to_pose_client_node_); action_server_->terminate_all(); return; } @@ -267,7 +292,7 @@ WaypointFollower::followWaypoints() "Processing waypoint %i...", goal_index); } - rclcpp::spin_some(client_node_); + rclcpp::spin_some(nav_to_pose_client_node_); r.sleep(); } } @@ -304,4 +329,174 @@ WaypointFollower::goalResponseCallback( } } +void WaypointFollower::followGPSWaypoints() +{ + auto goal = gps_action_server_->get_current_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + if (!gps_action_server_ || !gps_action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "GPS Action server inactive. Stopping."); + return; + } + + RCLCPP_INFO( + get_logger(), "Received follow GPS waypoint request with %i waypoints.", + static_cast(goal->waypoints.size())); + + std::vector poses = convertGPSWaypointstoPosesinMap( + goal->waypoints, shared_from_this(), from_ll_to_map_client_); + + auto is_action_server_ready = + waypoint_follower_action_client_->wait_for_action_server(std::chrono::seconds(5)); + if (!is_action_server_ready) { + RCLCPP_ERROR( + this->get_logger(), "FollowWaypoints action server is not available." + "make an instance of waypoint_follower is up and running"); + return; + } + waypoint_follower_goal_ = ClientTGPS::Goal(); + waypoint_follower_goal_.poses = poses; + + RCLCPP_INFO( + this->get_logger(), + "Sending a path of %zu waypoints:", waypoint_follower_goal_.poses.size()); + for (auto waypoint : waypoint_follower_goal_.poses) { + RCLCPP_DEBUG( + this->get_logger(), + "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); + } + + auto goal_options = ActionClientGPS::SendGoalOptions(); + goal_options.result_callback = std::bind( + &WaypointFollower::GPSResultCallback, this, + std::placeholders::_1); + goal_options.goal_response_callback = std::bind( + &WaypointFollower::GPSGoalResponseCallback, this, + std::placeholders::_1); + + auto future_goal_handle = waypoint_follower_action_client_->async_send_goal( + waypoint_follower_goal_, goal_options); + if (rclcpp::spin_until_future_complete( + waypoint_follower_client_node_, + future_goal_handle) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(get_logger(), "Send goal call failed"); + return; + } + // Get the goal handle and save so that we can check + // on completion in the timer callback + waypoint_follower_goal_handle_ = future_goal_handle.get(); + if (!waypoint_follower_goal_handle_) { + RCLCPP_ERROR(get_logger(), "Goal was rejected by server"); + return; + } +} + +std::vector +WaypointFollower::convertGPSWaypointstoPosesinMap( + const + std::vector & gps_waypoints, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, + const rclcpp::Client::SharedPtr & fromll_client) +{ + RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); + if (!parent_node) { + throw std::runtime_error{ + "Failed to lock node while trying to convert GPS waypoints to Map frame"}; + } + std::vector poses_in_map_frame_vector; + int index_of_gps_waypoints = 0; + for (auto && curr_gps_waypoint : gps_waypoints) { + auto fromLLRequest = std::make_shared(); + fromLLRequest->ll_point.latitude = curr_gps_waypoint.latitude; + fromLLRequest->ll_point.longitude = curr_gps_waypoint.longitude; + fromLLRequest->ll_point.altitude = curr_gps_waypoint.altitude; + if (!fromll_client->wait_for_service((std::chrono::seconds(10)))) { + RCLCPP_ERROR( + parent_node->get_logger(), + "fromLL service from robot_localization is not available" + "cannot convert GPS waypoints to Map frame poses," + "Make sure you have run navsat_transform_node of robot_localization"); + return std::vector(); + } + RCLCPP_INFO( + parent_node->get_logger(), "Processing for %i'th Waypoint..", + index_of_gps_waypoints); + auto inner_client_callback = + [&, parent_node](rclcpp::Client::SharedFuture inner_future) + { + auto result = inner_future.get(); + }; + auto inner_future_result = fromll_client->async_send_request( + fromLLRequest, + inner_client_callback); + // this poses are assumed to be on global frame (map) + geometry_msgs::msg::PoseStamped curr_waypoint_in_map_frame; + curr_waypoint_in_map_frame.header.frame_id = "map"; + curr_waypoint_in_map_frame.header.stamp = rclcpp::Clock().now(); + curr_waypoint_in_map_frame.pose.position.x = inner_future_result.get()->map_point.x; + curr_waypoint_in_map_frame.pose.position.y = inner_future_result.get()->map_point.y; + curr_waypoint_in_map_frame.pose.position.z = inner_future_result.get()->map_point.z; + + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, 0); + geometry_msgs::msg::Quaternion quat_msg; + tf2::convert(quat_msg, quat_tf); + curr_waypoint_in_map_frame.pose.orientation = quat_msg; + + RCLCPP_INFO( + parent_node->get_logger(), + "%i th Waypoint Long, Lat: %.8f , %.8f converted to " + "Map Point X,Y: %.8f , %.8f", index_of_gps_waypoints, fromLLRequest->ll_point.longitude, + fromLLRequest->ll_point.latitude, curr_waypoint_in_map_frame.pose.position.x, + curr_waypoint_in_map_frame.pose.position.y); + index_of_gps_waypoints++; + poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); + } + RCLCPP_INFO( + parent_node->get_logger(), + "Converted all %i GPS waypoint to Map frame", poses_in_map_frame_vector.size()); + return poses_in_map_frame_vector; +} + +void WaypointFollower::GPSResultCallback( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + gps_current_goal_status_ = ActionStatus::SUCCEEDED; + return; + case rclcpp_action::ResultCode::ABORTED: + gps_current_goal_status_ = ActionStatus::FAILED; + return; + case rclcpp_action::ResultCode::CANCELED: + gps_current_goal_status_ = ActionStatus::FAILED; + return; + default: + gps_current_goal_status_ = ActionStatus::UNKNOWN; + return; + } + + RCLCPP_INFO(this->get_logger(), "Result received"); + for (auto number : result.result->missed_waypoints) { + RCLCPP_INFO( + this->get_logger(), + "Missed" + "%d GPS waypoints", number); + } +} + +void WaypointFollower::GPSGoalResponseCallback( + const + rclcpp_action::ClientGoalHandle::SharedPtr & goal) +{ + if (!goal) { + RCLCPP_ERROR( + get_logger(), + "waypoint_follower action client failed to send goal to server."); + gps_current_goal_status_ = ActionStatus::FAILED; + } +} } // namespace nav2_waypoint_follower From 7b6f8cf4a1acad285eec747eea98cdb9b46f3543 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 7 Dec 2020 14:14:46 +0800 Subject: [PATCH 04/70] apply requested changes --- nav2_waypoint_follower/CMakeLists.txt | 2 +- nav2_waypoint_follower/README.md | 17 +- .../waypoint_follower.hpp | 77 +++--- .../src/waypoint_follower.cpp | 246 ++++++------------ 4 files changed, 124 insertions(+), 218 deletions(-) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 7f2e2cab58..14d3e1dc4f 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(nav2_util REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_core REQUIRED) find_package(pluginlib REQUIRED) -find_package(robot_localization REQUIRED) +find_package(robot_localization REQUIRED) nav2_package() diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index b5ad7baf05..66b403bd6a 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -27,18 +27,11 @@ Neither is better than the other, it highly depends on the tasks your robot(s) a ## Nav2 GPS Waypoint Follower -`nav2_waypoint_follower` provides an action server interface that accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself. The action for GPS waypoint following has properties as; - -```yaml -#goal definition -sensor_msgs/NavSatFix[] waypoints ---- -#result definition -int32[] missed_waypoints ---- -#feedback -uint32 current_waypoint -``` +`nav2_waypoint_follower` provides an action server named `FollowGPSWaypoints` which accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself. + +`robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude) +to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. +The action msg definition for GPS waypoint following can be found [here](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/action/FollowGPSWaypoints.action); In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. For instance; diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 75866e39a9..e7c50f43e2 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -61,11 +61,8 @@ class WaypointFollower : public nav2_util::LifecycleNode // Shorten the types for GPS waypoint following using ActionTGPS = nav2_msgs::action::FollowGPSWaypoints; - using ClientTGPS = nav2_msgs::action::FollowWaypoints; using ActionServerGPS = nav2_util::SimpleActionServer; - using ActionClientGPS = rclcpp_action::Client; - using WaypointFollowerGoalHandle = - rclcpp_action::ClientGoalHandle; + /** * @brief A constructor for nav2_waypoint_follower::WaypointFollower class */ @@ -110,21 +107,30 @@ class WaypointFollower : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; /** - * @brief Action server callbacks - */ - void followWaypoints(); - - /** - * @brief Action client result callback - * @param result Result of action server updated asynchronously + * @brief Templated function to perform internal logic behind waypoint following, + * Both GPS and non GPS waypoint following callbacks makes use of this function when a client asked to do so. + * Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback funtions for details. + * + * @tparam T action_server + * @tparam U poses + * @tparam V feedback + * @tparam Z result + * @param action_server + * @param poses + * @param feedback + * @param result */ - void resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + template + void followWaypointsLogic( + const T & action_server, + const U & poses, + const V & feedback, + const Z & result); /** - * @brief Action client goal response callback - * @param goal Response of action server updated asynchronously + * @brief Action server callbacks */ - void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + void followWaypointsCallback(); /** * @brief send robot through each of GPS @@ -133,7 +139,21 @@ class WaypointFollower : public nav2_util::LifecycleNode * * @param waypoints, acquired from action client */ - void followGPSWaypoints(); + void followGPSWaypointsCallback(); + + /** + * @brief Action client result callback + * @param result Result of action server updated asynchronously + */ + template + void resultCallback(const T & result); + + /** + * @brief Action client goal response callback + * @param goal Response of action server updated asynchronously + */ + template + void goalResponseCallback(const T & goal); /** * @brief given some gps_waypoints, converts them to map frame using robot_localization's service `fromLL`. @@ -150,36 +170,21 @@ class WaypointFollower : public nav2_util::LifecycleNode const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const rclcpp::Client::SharedPtr & fromll_client); - /** - * @brief Action client result callback - * @param result Result of action server updated asynchronously - */ - void GPSResultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); - - /** - * @brief Action client goal response callback - * @param goal Response of action server updated asynchronously - */ - void GPSGoalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + // Common vars used for both GPS and cartesian point following + rclcpp::Node::SharedPtr client_node_; + std::vector failed_ids_; + int loop_rate_; + bool stop_on_failure_; // Our action server for waypoint following std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; - rclcpp::Node::SharedPtr nav_to_pose_client_node_; std::shared_future::SharedPtr> future_goal_handle_; - bool stop_on_failure_; ActionStatus current_goal_status_; - int loop_rate_; - std::vector failed_ids_; // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; rclcpp::Client::SharedPtr from_ll_to_map_client_; - ActionClientGPS::SharedPtr waypoint_follower_action_client_; - rclcpp::Node::SharedPtr waypoint_follower_client_node_; - ActionStatus gps_current_goal_status_; - ClientTGPS::Goal waypoint_follower_goal_; - WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_; rclcpp::CallbackGroup::SharedPtr callback_group_; // Task Execution At Waypoint Plugin diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index f4685dcd30..be85a66b86 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -61,25 +61,18 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) new_args.push_back("-r"); new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); new_args.push_back("--"); - nav_to_pose_client_node_ = std::make_shared( + client_node_ = std::make_shared( "_", "", rclcpp::NodeOptions().arguments(new_args)); nav_to_pose_client_ = rclcpp_action::create_client( - nav_to_pose_client_node_, "navigate_to_pose"); + client_node_, "navigate_to_pose"); action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this)); - - // related to GPS waypoint following - waypoint_follower_client_node_ = std::make_shared( - "_", "", rclcpp::NodeOptions().arguments(new_args)); - - waypoint_follower_action_client_ = rclcpp_action::create_client( - waypoint_follower_client_node_, "FollowWaypoints"); + "FollowWaypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); callback_group_ = this->create_callback_group( rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); @@ -94,7 +87,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypoints, this)); + "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( this, @@ -150,7 +143,6 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_.reset(); nav_to_pose_client_.reset(); gps_action_server_.reset(); - waypoint_follower_action_client_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -162,25 +154,26 @@ WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void -WaypointFollower::followWaypoints() +template +void WaypointFollower::followWaypointsLogic( + const T & action_server, + const U & poses, + const V & feedback, + const Z & result) { - auto goal = action_server_->get_current_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); + auto goal = action_server->get_current_goal(); - // Check if request is valid - if (!action_server_ || !action_server_->is_server_active()) { + if (!action_server || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); return; } RCLCPP_INFO( get_logger(), "Received follow waypoint request with %i waypoints.", - static_cast(goal->poses.size())); + static_cast(poses.size())); - if (goal->poses.size() == 0) { - action_server_->succeeded_current(result); + if (poses.size() == 0) { + action_server->succeeded_current(result); return; } @@ -190,19 +183,19 @@ WaypointFollower::followWaypoints() while (rclcpp::ok()) { // Check if asked to stop processing action - if (action_server_->is_cancel_requested()) { + if (action_server->is_cancel_requested()) { auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); - rclcpp::spin_until_future_complete(nav_to_pose_client_node_, cancel_future); + rclcpp::spin_until_future_complete(client_node_, cancel_future); // for result callback processing - spin_some(nav_to_pose_client_node_); - action_server_->terminate_all(); + spin_some(client_node_); + action_server->terminate_all(); return; } // Check if asked to process another action - if (action_server_->is_preempt_requested()) { + if (action_server->is_preempt_requested()) { RCLCPP_INFO(get_logger(), "Preempting the goal pose."); - goal = action_server_->accept_pending_goal(); + goal = action_server->accept_pending_goal(); goal_index = 0; new_goal = true; } @@ -211,20 +204,28 @@ WaypointFollower::followWaypoints() if (new_goal) { new_goal = false; ClientT::Goal client_goal; - client_goal.pose = goal->poses[goal_index]; + client_goal.pose = poses[goal_index]; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = - std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1); + std::bind( + &WaypointFollower::resultCallback::WrappedResult>, + this, + std::placeholders::_1); send_goal_options.goal_response_callback = - std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); + std::bind( + &WaypointFollower::goalResponseCallback + ::SharedPtr>, this, + std::placeholders::_1); + future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); current_goal_status_ = ActionStatus::PROCESSING; } feedback->current_waypoint = goal_index; - action_server_->publish_feedback(feedback); + action_server->publish_feedback(feedback); if (current_goal_status_ == ActionStatus::FAILED) { failed_ids_.push_back(goal_index); @@ -235,7 +236,7 @@ WaypointFollower::followWaypoints() "list and stop on failure is enabled." " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; - action_server_->terminate_current(result); + action_server->terminate_current(result); failed_ids_.clear(); return; } else { @@ -248,7 +249,7 @@ WaypointFollower::followWaypoints() get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); bool is_task_executed = waypoint_task_executor_->processAtWaypoint( - goal->poses[goal_index], goal_index); + poses[goal_index], goal_index); RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); @@ -260,7 +261,7 @@ WaypointFollower::followWaypoints() " stop on failure is enabled." " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; - action_server_->terminate_current(result); + action_server->terminate_current(result); failed_ids_.clear(); return; } else { @@ -276,12 +277,12 @@ WaypointFollower::followWaypoints() // Update server state goal_index++; new_goal = true; - if (goal_index >= goal->poses.size()) { + if (goal_index >= poses.size()) { RCLCPP_INFO( get_logger(), "Completed all %i waypoints requested.", - goal->poses.size()); + poses.size()); result->missed_waypoints = failed_ids_; - action_server_->succeeded_current(result); + action_server->succeeded_current(result); failed_ids_.clear(); return; } @@ -291,15 +292,42 @@ WaypointFollower::followWaypoints() (static_cast(now().seconds()) % 30 == 0), "Processing waypoint %i...", goal_index); } - - rclcpp::spin_some(nav_to_pose_client_node_); + rclcpp::spin_some(client_node_); r.sleep(); } } -void -WaypointFollower::resultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result) +void WaypointFollower::followWaypointsCallback() +{ + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + followWaypointsLogic, + std::vector, ActionT::Feedback::SharedPtr, + ActionT::Result::SharedPtr>( + action_server_, + action_server_->get_current_goal()->poses, + feedback, result); +} + +void WaypointFollower::followGPSWaypointsCallback() +{ + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + std::vector poses = convertGPSWaypointstoPosesinMap( + gps_action_server_->get_current_goal()->waypoints, shared_from_this(), from_ll_to_map_client_); + + followWaypointsLogic, + std::vector, ActionTGPS::Feedback::SharedPtr, + ActionTGPS::Result::SharedPtr>( + gps_action_server_, + poses, feedback, result); +} + +template +void WaypointFollower::resultCallback( + const T & result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: @@ -317,9 +345,9 @@ WaypointFollower::resultCallback( } } -void -WaypointFollower::goalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal) +template +void WaypointFollower::goalResponseCallback( + const T & goal) { if (!goal) { RCLCPP_ERROR( @@ -329,69 +357,6 @@ WaypointFollower::goalResponseCallback( } } -void WaypointFollower::followGPSWaypoints() -{ - auto goal = gps_action_server_->get_current_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - if (!gps_action_server_ || !gps_action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "GPS Action server inactive. Stopping."); - return; - } - - RCLCPP_INFO( - get_logger(), "Received follow GPS waypoint request with %i waypoints.", - static_cast(goal->waypoints.size())); - - std::vector poses = convertGPSWaypointstoPosesinMap( - goal->waypoints, shared_from_this(), from_ll_to_map_client_); - - auto is_action_server_ready = - waypoint_follower_action_client_->wait_for_action_server(std::chrono::seconds(5)); - if (!is_action_server_ready) { - RCLCPP_ERROR( - this->get_logger(), "FollowWaypoints action server is not available." - "make an instance of waypoint_follower is up and running"); - return; - } - waypoint_follower_goal_ = ClientTGPS::Goal(); - waypoint_follower_goal_.poses = poses; - - RCLCPP_INFO( - this->get_logger(), - "Sending a path of %zu waypoints:", waypoint_follower_goal_.poses.size()); - for (auto waypoint : waypoint_follower_goal_.poses) { - RCLCPP_DEBUG( - this->get_logger(), - "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); - } - - auto goal_options = ActionClientGPS::SendGoalOptions(); - goal_options.result_callback = std::bind( - &WaypointFollower::GPSResultCallback, this, - std::placeholders::_1); - goal_options.goal_response_callback = std::bind( - &WaypointFollower::GPSGoalResponseCallback, this, - std::placeholders::_1); - - auto future_goal_handle = waypoint_follower_action_client_->async_send_goal( - waypoint_follower_goal_, goal_options); - if (rclcpp::spin_until_future_complete( - waypoint_follower_client_node_, - future_goal_handle) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(get_logger(), "Send goal call failed"); - return; - } - // Get the goal handle and save so that we can check - // on completion in the timer callback - waypoint_follower_goal_handle_ = future_goal_handle.get(); - if (!waypoint_follower_goal_handle_) { - RCLCPP_ERROR(get_logger(), "Goal was rejected by server"); - return; - } -} std::vector WaypointFollower::convertGPSWaypointstoPosesinMap( @@ -401,10 +366,7 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( const rclcpp::Client::SharedPtr & fromll_client) { RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); - if (!parent_node) { - throw std::runtime_error{ - "Failed to lock node while trying to convert GPS waypoints to Map frame"}; - } + std::vector poses_in_map_frame_vector; int index_of_gps_waypoints = 0; for (auto && curr_gps_waypoint : gps_waypoints) { @@ -412,7 +374,7 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( fromLLRequest->ll_point.latitude = curr_gps_waypoint.latitude; fromLLRequest->ll_point.longitude = curr_gps_waypoint.longitude; fromLLRequest->ll_point.altitude = curr_gps_waypoint.altitude; - if (!fromll_client->wait_for_service((std::chrono::seconds(10)))) { + if (!fromll_client->wait_for_service((std::chrono::seconds(1)))) { RCLCPP_ERROR( parent_node->get_logger(), "fromLL service from robot_localization is not available" @@ -420,9 +382,6 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( "Make sure you have run navsat_transform_node of robot_localization"); return std::vector(); } - RCLCPP_INFO( - parent_node->get_logger(), "Processing for %i'th Waypoint..", - index_of_gps_waypoints); auto inner_client_callback = [&, parent_node](rclcpp::Client::SharedFuture inner_future) { @@ -434,23 +393,11 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( // this poses are assumed to be on global frame (map) geometry_msgs::msg::PoseStamped curr_waypoint_in_map_frame; curr_waypoint_in_map_frame.header.frame_id = "map"; - curr_waypoint_in_map_frame.header.stamp = rclcpp::Clock().now(); + curr_waypoint_in_map_frame.header.stamp = parent_node->now(); curr_waypoint_in_map_frame.pose.position.x = inner_future_result.get()->map_point.x; curr_waypoint_in_map_frame.pose.position.y = inner_future_result.get()->map_point.y; curr_waypoint_in_map_frame.pose.position.z = inner_future_result.get()->map_point.z; - tf2::Quaternion quat_tf; - quat_tf.setRPY(0, 0, 0); - geometry_msgs::msg::Quaternion quat_msg; - tf2::convert(quat_msg, quat_tf); - curr_waypoint_in_map_frame.pose.orientation = quat_msg; - - RCLCPP_INFO( - parent_node->get_logger(), - "%i th Waypoint Long, Lat: %.8f , %.8f converted to " - "Map Point X,Y: %.8f , %.8f", index_of_gps_waypoints, fromLLRequest->ll_point.longitude, - fromLLRequest->ll_point.latitude, curr_waypoint_in_map_frame.pose.position.x, - curr_waypoint_in_map_frame.pose.position.y); index_of_gps_waypoints++; poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); } @@ -460,43 +407,4 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( return poses_in_map_frame_vector; } -void WaypointFollower::GPSResultCallback( - const rclcpp_action::ClientGoalHandle - ::WrappedResult & result) -{ - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - gps_current_goal_status_ = ActionStatus::SUCCEEDED; - return; - case rclcpp_action::ResultCode::ABORTED: - gps_current_goal_status_ = ActionStatus::FAILED; - return; - case rclcpp_action::ResultCode::CANCELED: - gps_current_goal_status_ = ActionStatus::FAILED; - return; - default: - gps_current_goal_status_ = ActionStatus::UNKNOWN; - return; - } - - RCLCPP_INFO(this->get_logger(), "Result received"); - for (auto number : result.result->missed_waypoints) { - RCLCPP_INFO( - this->get_logger(), - "Missed" - "%d GPS waypoints", number); - } -} - -void WaypointFollower::GPSGoalResponseCallback( - const - rclcpp_action::ClientGoalHandle::SharedPtr & goal) -{ - if (!goal) { - RCLCPP_ERROR( - get_logger(), - "waypoint_follower action client failed to send goal to server."); - gps_current_goal_status_ = ActionStatus::FAILED; - } -} } // namespace nav2_waypoint_follower From ac94d2d9d4552ba8362cd428cfe2a55c96f72a70 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 7 Dec 2020 15:16:53 +0800 Subject: [PATCH 05/70] nav2_util::ServiceClient instead of CallbackGroup --- .../waypoint_follower.hpp | 19 +++--- .../src/waypoint_follower.cpp | 61 +++++++++---------- 2 files changed, 38 insertions(+), 42 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index e7c50f43e2..6bccb9bd07 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -19,19 +19,21 @@ #include #include +#include "rclcpp_action/rclcpp_action.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/simple_action_server.hpp" -#include "rclcpp_action/rclcpp_action.hpp" - #include "nav2_util/node_utils.hpp" -#include "nav2_core/waypoint_task_executor.hpp" -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" #include "nav2_msgs/action/follow_gps_waypoints.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_core/waypoint_task_executor.hpp" + #include "robot_localization/srv/from_ll.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -168,7 +170,7 @@ class WaypointFollower : public nav2_util::LifecycleNode const std::vector & gps_waypoints, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - const rclcpp::Client::SharedPtr & fromll_client); + nav2_util::ServiceClient & fromll_client); // Common vars used for both GPS and cartesian point following rclcpp::Node::SharedPtr client_node_; @@ -184,8 +186,7 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; - rclcpp::Client::SharedPtr from_ll_to_map_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; + nav2_util::ServiceClient from_ll_to_map_client_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index be85a66b86..03930051b9 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -26,6 +26,7 @@ namespace nav2_waypoint_follower WaypointFollower::WaypointFollower() : nav2_util::LifecycleNode("WaypointFollower", "", false), + from_ll_to_map_client_("/fromLL", client_node_), waypoint_task_executor_loader_("nav2_waypoint_follower", "nav2_core::WaypointTaskExecutor") { @@ -74,13 +75,9 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "FollowWaypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); - callback_group_ = this->create_callback_group( - rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); - from_ll_to_map_client_ = - this->create_client( + from_ll_to_map_client_ = nav2_util::ServiceClient( "/fromLL", - rmw_qos_profile_services_default, - callback_group_); + client_node_); gps_action_server_ = std::make_unique( get_node_base_interface(), @@ -363,43 +360,41 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( const std::vector & gps_waypoints, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - const rclcpp::Client::SharedPtr & fromll_client) + nav2_util::ServiceClient & fromll_client) { RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); std::vector poses_in_map_frame_vector; int index_of_gps_waypoints = 0; for (auto && curr_gps_waypoint : gps_waypoints) { - auto fromLLRequest = std::make_shared(); - fromLLRequest->ll_point.latitude = curr_gps_waypoint.latitude; - fromLLRequest->ll_point.longitude = curr_gps_waypoint.longitude; - fromLLRequest->ll_point.altitude = curr_gps_waypoint.altitude; - if (!fromll_client->wait_for_service((std::chrono::seconds(1)))) { + auto request = std::make_shared(); + auto response = std::make_shared(); + request->ll_point.latitude = curr_gps_waypoint.latitude; + request->ll_point.longitude = curr_gps_waypoint.longitude; + request->ll_point.altitude = curr_gps_waypoint.altitude; + fromll_client.wait_for_service((std::chrono::seconds(1))); + auto is_conversion_succeeded = fromll_client.invoke( + request, + response); + if (!is_conversion_succeeded) { RCLCPP_ERROR( parent_node->get_logger(), - "fromLL service from robot_localization is not available" - "cannot convert GPS waypoints to Map frame poses," + "fromLL service of robot_localization could not convert %i th GPS waypoint to" + "Map frame, going to skip this point!" "Make sure you have run navsat_transform_node of robot_localization"); - return std::vector(); + index_of_gps_waypoints++; + continue; + } else { + // this poses are assumed to be on global frame (map) + geometry_msgs::msg::PoseStamped curr_waypoint_in_map_frame; + curr_waypoint_in_map_frame.header.frame_id = "map"; + curr_waypoint_in_map_frame.header.stamp = parent_node->now(); + curr_waypoint_in_map_frame.pose.position.x = response->map_point.x; + curr_waypoint_in_map_frame.pose.position.y = response->map_point.y; + curr_waypoint_in_map_frame.pose.position.z = response->map_point.z; + index_of_gps_waypoints++; + poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); } - auto inner_client_callback = - [&, parent_node](rclcpp::Client::SharedFuture inner_future) - { - auto result = inner_future.get(); - }; - auto inner_future_result = fromll_client->async_send_request( - fromLLRequest, - inner_client_callback); - // this poses are assumed to be on global frame (map) - geometry_msgs::msg::PoseStamped curr_waypoint_in_map_frame; - curr_waypoint_in_map_frame.header.frame_id = "map"; - curr_waypoint_in_map_frame.header.stamp = parent_node->now(); - curr_waypoint_in_map_frame.pose.position.x = inner_future_result.get()->map_point.x; - curr_waypoint_in_map_frame.pose.position.y = inner_future_result.get()->map_point.y; - curr_waypoint_in_map_frame.pose.position.z = inner_future_result.get()->map_point.z; - - index_of_gps_waypoints++; - poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); } RCLCPP_INFO( parent_node->get_logger(), From cd9a287063c0f187ef5a544f1d893278f9e67040 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Tue, 8 Dec 2020 16:54:17 +0800 Subject: [PATCH 06/70] another iteration to adress issues --- nav2_waypoint_follower/CMakeLists.txt | 5 ++ nav2_waypoint_follower/README.md | 4 +- .../waypoint_follower.hpp | 12 ++--- .../src/waypoint_follower.cpp | 52 ++++++++++++------- 4 files changed, 44 insertions(+), 29 deletions(-) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 14d3e1dc4f..2a43e68751 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,6 +1,11 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + # Try for OpenCV 4.X, but settle for whatever is installed find_package(OpenCV 4 QUIET) if(NOT OpenCV_FOUND) diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 66b403bd6a..87a469be80 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -31,10 +31,10 @@ Neither is better than the other, it highly depends on the tasks your robot(s) a `robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude) to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. -The action msg definition for GPS waypoint following can be found [here](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/action/FollowGPSWaypoints.action); +The action msg definition for GPS waypoint following can be found [here](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/action/FollowGPSWaypoints.action). In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. -For instance; +For instance, ```cpp using ClientT = nav2_msgs::action::FollowGPSWaypoints; diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 6bccb9bd07..79f498cded 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -114,7 +114,6 @@ class WaypointFollower : public nav2_util::LifecycleNode * Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback funtions for details. * * @tparam T action_server - * @tparam U poses * @tparam V feedback * @tparam Z result * @param action_server @@ -122,10 +121,9 @@ class WaypointFollower : public nav2_util::LifecycleNode * @param feedback * @param result */ - template + template void followWaypointsLogic( const T & action_server, - const U & poses, const V & feedback, const Z & result); @@ -167,10 +165,10 @@ class WaypointFollower : public nav2_util::LifecycleNode * @return std::vector */ static std::vector convertGPSWaypointstoPosesinMap( - const - std::vector & gps_waypoints, + const std::vector & gps_waypoints, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - nav2_util::ServiceClient & fromll_client); + const + std::unique_ptr> & fromll_client); // Common vars used for both GPS and cartesian point following rclcpp::Node::SharedPtr client_node_; @@ -186,7 +184,7 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; - nav2_util::ServiceClient from_ll_to_map_client_; + std::unique_ptr> from_ll_to_map_client_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 03930051b9..bd589be401 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -26,7 +26,6 @@ namespace nav2_waypoint_follower WaypointFollower::WaypointFollower() : nav2_util::LifecycleNode("WaypointFollower", "", false), - from_ll_to_map_client_("/fromLL", client_node_), waypoint_task_executor_loader_("nav2_waypoint_follower", "nav2_core::WaypointTaskExecutor") { @@ -75,7 +74,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "FollowWaypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); - from_ll_to_map_client_ = nav2_util::ServiceClient( + from_ll_to_map_client_ = std::make_unique< + nav2_util::ServiceClient>( "/fromLL", client_node_); @@ -151,15 +151,28 @@ WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -template +template void WaypointFollower::followWaypointsLogic( const T & action_server, - const U & poses, const V & feedback, const Z & result) { auto goal = action_server->get_current_goal(); + std::vector poses; + + // compile time static check to decide which block of code to be built + if constexpr (std::is_same>::value) + { + // If normal waypoint following callback was called, we build here + poses = goal->poses; + } else { + // If GPS waypoint following callback was called, we build here + poses = convertGPSWaypointstoPosesinMap( + goal->waypoints, shared_from_this(), + from_ll_to_map_client_); + } + if (!action_server || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); return; @@ -193,6 +206,14 @@ void WaypointFollower::followWaypointsLogic( if (action_server->is_preempt_requested()) { RCLCPP_INFO(get_logger(), "Preempting the goal pose."); goal = action_server->accept_pending_goal(); + if constexpr (std::is_same>::value) + { + poses = goal->poses; // Discarded if cond is false + } else { + poses = convertGPSWaypointstoPosesinMap( + goal->waypoints, shared_from_this(), + from_ll_to_map_client_); + } goal_index = 0; new_goal = true; } @@ -300,10 +321,9 @@ void WaypointFollower::followWaypointsCallback() auto result = std::make_shared(); followWaypointsLogic, - std::vector, ActionT::Feedback::SharedPtr, + ActionT::Feedback::SharedPtr, ActionT::Result::SharedPtr>( action_server_, - action_server_->get_current_goal()->poses, feedback, result); } @@ -312,14 +332,11 @@ void WaypointFollower::followGPSWaypointsCallback() auto feedback = std::make_shared(); auto result = std::make_shared(); - std::vector poses = convertGPSWaypointstoPosesinMap( - gps_action_server_->get_current_goal()->waypoints, shared_from_this(), from_ll_to_map_client_); - followWaypointsLogic, - std::vector, ActionTGPS::Feedback::SharedPtr, + ActionTGPS::Feedback::SharedPtr, ActionTGPS::Result::SharedPtr>( gps_action_server_, - poses, feedback, result); + feedback, result); } template @@ -354,26 +371,23 @@ void WaypointFollower::goalResponseCallback( } } - std::vector WaypointFollower::convertGPSWaypointstoPosesinMap( - const - std::vector & gps_waypoints, + const std::vector & gps_waypoints, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - nav2_util::ServiceClient & fromll_client) + const std::unique_ptr> & fromll_client) { RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); std::vector poses_in_map_frame_vector; - int index_of_gps_waypoints = 0; for (auto && curr_gps_waypoint : gps_waypoints) { auto request = std::make_shared(); auto response = std::make_shared(); request->ll_point.latitude = curr_gps_waypoint.latitude; request->ll_point.longitude = curr_gps_waypoint.longitude; request->ll_point.altitude = curr_gps_waypoint.altitude; - fromll_client.wait_for_service((std::chrono::seconds(1))); - auto is_conversion_succeeded = fromll_client.invoke( + fromll_client->wait_for_service((std::chrono::seconds(1))); + auto is_conversion_succeeded = fromll_client->invoke( request, response); if (!is_conversion_succeeded) { @@ -382,7 +396,6 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( "fromLL service of robot_localization could not convert %i th GPS waypoint to" "Map frame, going to skip this point!" "Make sure you have run navsat_transform_node of robot_localization"); - index_of_gps_waypoints++; continue; } else { // this poses are assumed to be on global frame (map) @@ -392,7 +405,6 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( curr_waypoint_in_map_frame.pose.position.x = response->map_point.x; curr_waypoint_in_map_frame.pose.position.y = response->map_point.y; curr_waypoint_in_map_frame.pose.position.z = response->map_point.z; - index_of_gps_waypoints++; poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); } } From 9c0e4085d9df3e8ca7bb572efa8aa8602482d0da Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 9 Dec 2020 04:53:17 +0800 Subject: [PATCH 07/70] update poses with function in the follower logic --- .../waypoint_follower.hpp | 3 ++ .../src/waypoint_follower.cpp | 37 ++++++++++--------- tools/underlay.repos | 4 ++ 3 files changed, 27 insertions(+), 17 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 79f498cded..6642030ffa 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -170,6 +170,9 @@ class WaypointFollower : public nav2_util::LifecycleNode const std::unique_ptr> & fromll_client); + template + std::vector getUpdatedPoses(const T & action_server); + // Common vars used for both GPS and cartesian point following rclcpp::Node::SharedPtr client_node_; std::vector failed_ids_; diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index bd589be401..2ac951da02 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -151,27 +151,37 @@ WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -template -void WaypointFollower::followWaypointsLogic( - const T & action_server, - const V & feedback, - const Z & result) +template +std::vector WaypointFollower::getUpdatedPoses( + const T & action_server) { - auto goal = action_server->get_current_goal(); - std::vector poses; // compile time static check to decide which block of code to be built if constexpr (std::is_same>::value) { // If normal waypoint following callback was called, we build here - poses = goal->poses; + poses = action_server->get_current_goal()->poses; } else { // If GPS waypoint following callback was called, we build here poses = convertGPSWaypointstoPosesinMap( - goal->waypoints, shared_from_this(), + action_server->get_current_goal()->waypoints, shared_from_this(), from_ll_to_map_client_); } + return poses; +} + +template +void WaypointFollower::followWaypointsLogic( + const T & action_server, + const V & feedback, + const Z & result) +{ + auto goal = action_server->get_current_goal(); + + std::vector poses; + + poses = getUpdatedPoses(action_server); if (!action_server || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); @@ -206,14 +216,7 @@ void WaypointFollower::followWaypointsLogic( if (action_server->is_preempt_requested()) { RCLCPP_INFO(get_logger(), "Preempting the goal pose."); goal = action_server->accept_pending_goal(); - if constexpr (std::is_same>::value) - { - poses = goal->poses; // Discarded if cond is false - } else { - poses = convertGPSWaypointstoPosesinMap( - goal->waypoints, shared_from_this(), - from_ll_to_map_client_); - } + poses = getUpdatedPoses(action_server); goal_index = 0; new_goal = true; } diff --git a/tools/underlay.repos b/tools/underlay.repos index 6df91bf25f..0da86285d8 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -27,3 +27,7 @@ repositories: type: git url: https://github.com/ompl/ompl.git version: 1.5.0 + robot_localization/robot_localization: + type: git + url: https://github.com/cra-ros-pkg/robot_localization.git + version: foxy-devel \ No newline at end of file From f94e3b352343c8d10f639a27807189431b4e5649 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 9 Dec 2020 05:00:06 +0800 Subject: [PATCH 08/70] add deps of robot_localization: diagnostics --- tools/underlay.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tools/underlay.repos b/tools/underlay.repos index 0da86285d8..0792665d0a 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -23,6 +23,10 @@ repositories: type: git url: https://github.com/stevemacenski/bond_core.git version: fix_deprecation_warning + ros/ + type: git + url: https://github.com/ros/diagnostics.git + version: ros2-devel ompl/ompl: type: git url: https://github.com/ompl/ompl.git From e4fe18413d8dd881983579905bf31bad9aa6c775 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 9 Dec 2020 05:01:28 +0800 Subject: [PATCH 09/70] fix typo in underlay.repo --- tools/underlay.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 0792665d0a..36af5a99c2 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -23,7 +23,7 @@ repositories: type: git url: https://github.com/stevemacenski/bond_core.git version: fix_deprecation_warning - ros/ + ros/diagnostics: type: git url: https://github.com/ros/diagnostics.git version: ros2-devel From c2f4d95d6f53df101c562ad0adbb1e91638a397d Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 9 Dec 2020 05:09:24 +0800 Subject: [PATCH 10/70] add deps of robot_localization: geographic_info --- tools/underlay.repos | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 36af5a99c2..47d9fae14b 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -26,7 +26,11 @@ repositories: ros/diagnostics: type: git url: https://github.com/ros/diagnostics.git - version: ros2-devel + version: ros2-devel + ros/geographic_info: + type: git + url: https://github.com/ros-geographic-info/geographic_info.git + version: ros2 ompl/ompl: type: git url: https://github.com/ompl/ompl.git From 98804f16fc29b6aa0cbbe6d44a6045bb370a889e Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 9 Dec 2020 06:05:51 +0800 Subject: [PATCH 11/70] minor clean-ups --- nav2_waypoint_follower/src/waypoint_follower.cpp | 6 +++--- tools/underlay.repos | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 2ac951da02..4c528c1d32 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -55,7 +55,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); - // related to waypoint following + std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); new_args.push_back("-r"); @@ -85,6 +85,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_logging_interface(), get_node_waitables_interface(), "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); + try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( this, @@ -140,6 +141,7 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_.reset(); nav_to_pose_client_.reset(); gps_action_server_.reset(); + from_ll_to_map_client_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -180,7 +182,6 @@ void WaypointFollower::followWaypointsLogic( auto goal = action_server->get_current_goal(); std::vector poses; - poses = getUpdatedPoses(action_server); if (!action_server || !action_server->is_server_active()) { @@ -228,7 +229,6 @@ void WaypointFollower::followWaypointsLogic( client_goal.pose = poses[goal_index]; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = std::bind( &WaypointFollower::resultCallback::WrappedResult>, diff --git a/tools/underlay.repos b/tools/underlay.repos index 47d9fae14b..cc35927387 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -38,4 +38,5 @@ repositories: robot_localization/robot_localization: type: git url: https://github.com/cra-ros-pkg/robot_localization.git - version: foxy-devel \ No newline at end of file + version: foxy-devel + \ No newline at end of file From 960bb13c49851668893ea546f60765fc5819c635 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 9 Dec 2020 11:46:16 +0800 Subject: [PATCH 12/70] bond_core version has been updated --- tools/underlay.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index cc35927387..ff4b2d2568 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -21,8 +21,8 @@ repositories: version: ros2 ros/bond_core: type: git - url: https://github.com/stevemacenski/bond_core.git - version: fix_deprecation_warning + url: https://github.com/ros/bond_core.git + version: ros2 ros/diagnostics: type: git url: https://github.com/ros/diagnostics.git From 623c6a665dfb13b8efabb5ba424c58dcc22323df Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 14 Dec 2020 13:24:04 +0800 Subject: [PATCH 13/70] rotation should also be considered in GPS WPFing --- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/action/FollowGPSWaypoints.action | 2 +- nav2_msgs/msg/OrientedNavSatFix.msg | 2 ++ nav2_waypoint_follower/CMakeLists.txt | 2 ++ .../waypoint_follower.hpp | 19 +++++++++---- nav2_waypoint_follower/package.xml | 1 + .../src/waypoint_follower.cpp | 28 +++++++++++++++---- 7 files changed, 42 insertions(+), 13 deletions(-) create mode 100644 nav2_msgs/msg/OrientedNavSatFix.msg diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index c770c1364c..99c4c93f67 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/OrientedNavSatFix.msg" "srv/GetCostmap.srv" "srv/ClearCostmapExceptRegion.srv" "srv/ClearCostmapAroundRobot.srv" diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action index d83a59170a..45a24ae7f3 100644 --- a/nav2_msgs/action/FollowGPSWaypoints.action +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -1,5 +1,5 @@ #goal definition -sensor_msgs/NavSatFix[] waypoints +nav2_msgs/OrientedNavSatFix[] gps_poses --- #result definition int32[] missed_waypoints diff --git a/nav2_msgs/msg/OrientedNavSatFix.msg b/nav2_msgs/msg/OrientedNavSatFix.msg new file mode 100644 index 0000000000..aa5b024ddb --- /dev/null +++ b/nav2_msgs/msg/OrientedNavSatFix.msg @@ -0,0 +1,2 @@ +sensor_msgs/NavSatFix position +geometry_msgs/Quaternion orientation \ No newline at end of file diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 2a43e68751..d3c0afa224 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav_2d_utils REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_core REQUIRED) find_package(pluginlib REQUIRED) @@ -53,6 +54,7 @@ set(dependencies nav_msgs nav2_msgs nav2_util + nav_2d_utils tf2_ros nav2_core pluginlib diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 6642030ffa..3a7a348afa 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -22,8 +22,7 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" - +#include "nav2_msgs/msg/oriented_nav_sat_fix.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" @@ -36,6 +35,9 @@ #include "robot_localization/srv/from_ll.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "tf2/transform_datatypes.h" namespace nav2_waypoint_follower { @@ -156,16 +158,16 @@ class WaypointFollower : public nav2_util::LifecycleNode void goalResponseCallback(const T & goal); /** - * @brief given some gps_waypoints, converts them to map frame using robot_localization's service `fromLL`. + * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. * Constructs a vector of stamped poses in map frame and returns them. * - * @param gps_waypoints + * @param gps_poses * @param parent_node * @param fromll_client * @return std::vector */ - static std::vector convertGPSWaypointstoPosesinMap( - const std::vector & gps_waypoints, + std::vector convertGPSWaypointstoPosesinMap( + const std::vector & gps_waypoints, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const std::unique_ptr> & fromll_client); @@ -196,6 +198,11 @@ class WaypointFollower : public nav2_util::LifecycleNode waypoint_task_executor_; std::string waypoint_task_executor_id_; std::string waypoint_task_executor_type_; + + // tf buffer to get transfroms + std::shared_ptr tf_buffer_; + // tf listner for tf transforms + std::shared_ptr tf_listener_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index f16494af58..0ed3197310 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -19,6 +19,7 @@ nav_msgs nav2_msgs nav2_util + nav_2d_utils nav2_core tf2_ros robot_localization diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 4c528c1d32..583d4cd5a5 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "nav2_waypoint_follower/waypoint_follower.hpp" +#include "nav_2d_utils/tf_help.hpp" #include #include @@ -85,6 +86,9 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_logging_interface(), get_node_waitables_interface(), "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); + // used for transfroming orientation of GPS poses to map frame + tf_buffer_ = std::make_shared(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( @@ -167,7 +171,7 @@ std::vector WaypointFollower::getUpdatedPoses( } else { // If GPS waypoint following callback was called, we build here poses = convertGPSWaypointstoPosesinMap( - action_server->get_current_goal()->waypoints, shared_from_this(), + action_server->get_current_goal()->gps_poses, shared_from_this(), from_ll_to_map_client_); } return poses; @@ -376,19 +380,22 @@ void WaypointFollower::goalResponseCallback( std::vector WaypointFollower::convertGPSWaypointstoPosesinMap( - const std::vector & gps_waypoints, + const std::vector & gps_poses, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const std::unique_ptr> & fromll_client) { RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); + rclcpp::Duration transform_tolerance(0, 500); std::vector poses_in_map_frame_vector; - for (auto && curr_gps_waypoint : gps_waypoints) { + for (auto && curr_gps_waypoint : gps_poses) { + auto request = std::make_shared(); auto response = std::make_shared(); - request->ll_point.latitude = curr_gps_waypoint.latitude; - request->ll_point.longitude = curr_gps_waypoint.longitude; - request->ll_point.altitude = curr_gps_waypoint.altitude; + request->ll_point.latitude = curr_gps_waypoint.position.latitude; + request->ll_point.longitude = curr_gps_waypoint.position.longitude; + request->ll_point.altitude = curr_gps_waypoint.position.altitude; + fromll_client->wait_for_service((std::chrono::seconds(1))); auto is_conversion_succeeded = fromll_client->invoke( request, @@ -408,6 +415,15 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( curr_waypoint_in_map_frame.pose.position.x = response->map_point.x; curr_waypoint_in_map_frame.pose.position.y = response->map_point.y; curr_waypoint_in_map_frame.pose.position.z = response->map_point.z; + + geometry_msgs::msg::Quaternion utm_orientation = curr_gps_waypoint.orientation; + geometry_msgs::msg::PoseStamped temporary_pose; + temporary_pose.pose.orientation = utm_orientation; + temporary_pose.header.frame_id = "utm"; + nav_2d_utils::transformPose( + tf_buffer_, "map", temporary_pose, temporary_pose, transform_tolerance); + curr_waypoint_in_map_frame.pose.orientation = temporary_pose.pose.orientation; + poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); } } From ef57df0d4d86be01a8d6df586e3a6d2c7d4de04d Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 14 Dec 2020 13:36:38 +0800 Subject: [PATCH 14/70] use better namings related to gps wpf orientation --- .../waypoint_follower.hpp | 11 ++---- .../src/waypoint_follower.cpp | 39 +++++++++---------- 2 files changed, 22 insertions(+), 28 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 3a7a348afa..c9a823fdab 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -34,10 +34,7 @@ #include "nav2_core/waypoint_task_executor.hpp" #include "robot_localization/srv/from_ll.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" -#include "tf2/transform_datatypes.h" namespace nav2_waypoint_follower { @@ -166,14 +163,14 @@ class WaypointFollower : public nav2_util::LifecycleNode * @param fromll_client * @return std::vector */ - std::vector convertGPSWaypointstoPosesinMap( - const std::vector & gps_waypoints, + std::vector convertGPSPoses2MapPoses( + const std::vector & gps_poses, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const std::unique_ptr> & fromll_client); template - std::vector getUpdatedPoses(const T & action_server); + std::vector getLatestGoalPoses(const T & action_server); // Common vars used for both GPS and cartesian point following rclcpp::Node::SharedPtr client_node_; @@ -201,8 +198,6 @@ class WaypointFollower : public nav2_util::LifecycleNode // tf buffer to get transfroms std::shared_ptr tf_buffer_; - // tf listner for tf transforms - std::shared_ptr tf_listener_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 583d4cd5a5..573a8854ef 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -88,7 +88,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); // used for transfroming orientation of GPS poses to map frame tf_buffer_ = std::make_shared(node->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( @@ -158,7 +157,7 @@ WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) } template -std::vector WaypointFollower::getUpdatedPoses( +std::vector WaypointFollower::getLatestGoalPoses( const T & action_server) { std::vector poses; @@ -170,7 +169,7 @@ std::vector WaypointFollower::getUpdatedPoses( poses = action_server->get_current_goal()->poses; } else { // If GPS waypoint following callback was called, we build here - poses = convertGPSWaypointstoPosesinMap( + poses = convertGPSPoses2MapPoses( action_server->get_current_goal()->gps_poses, shared_from_this(), from_ll_to_map_client_); } @@ -186,7 +185,7 @@ void WaypointFollower::followWaypointsLogic( auto goal = action_server->get_current_goal(); std::vector poses; - poses = getUpdatedPoses(action_server); + poses = getLatestGoalPoses(action_server); if (!action_server || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); @@ -221,7 +220,7 @@ void WaypointFollower::followWaypointsLogic( if (action_server->is_preempt_requested()) { RCLCPP_INFO(get_logger(), "Preempting the goal pose."); goal = action_server->accept_pending_goal(); - poses = getUpdatedPoses(action_server); + poses = getLatestGoalPoses(action_server); goal_index = 0; new_goal = true; } @@ -379,7 +378,7 @@ void WaypointFollower::goalResponseCallback( } std::vector -WaypointFollower::convertGPSWaypointstoPosesinMap( +WaypointFollower::convertGPSPoses2MapPoses( const std::vector & gps_poses, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const std::unique_ptr> & fromll_client) @@ -388,13 +387,13 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( rclcpp::Duration transform_tolerance(0, 500); std::vector poses_in_map_frame_vector; - for (auto && curr_gps_waypoint : gps_poses) { + for (auto && curr_gps_pose : gps_poses) { auto request = std::make_shared(); auto response = std::make_shared(); - request->ll_point.latitude = curr_gps_waypoint.position.latitude; - request->ll_point.longitude = curr_gps_waypoint.position.longitude; - request->ll_point.altitude = curr_gps_waypoint.position.altitude; + request->ll_point.latitude = curr_gps_pose.position.latitude; + request->ll_point.longitude = curr_gps_pose.position.longitude; + request->ll_point.altitude = curr_gps_pose.position.altitude; fromll_client->wait_for_service((std::chrono::seconds(1))); auto is_conversion_succeeded = fromll_client->invoke( @@ -409,22 +408,22 @@ WaypointFollower::convertGPSWaypointstoPosesinMap( continue; } else { // this poses are assumed to be on global frame (map) - geometry_msgs::msg::PoseStamped curr_waypoint_in_map_frame; - curr_waypoint_in_map_frame.header.frame_id = "map"; - curr_waypoint_in_map_frame.header.stamp = parent_node->now(); - curr_waypoint_in_map_frame.pose.position.x = response->map_point.x; - curr_waypoint_in_map_frame.pose.position.y = response->map_point.y; - curr_waypoint_in_map_frame.pose.position.z = response->map_point.z; - - geometry_msgs::msg::Quaternion utm_orientation = curr_gps_waypoint.orientation; + geometry_msgs::msg::PoseStamped curr_pose_in_map_frame; + curr_pose_in_map_frame.header.frame_id = "map"; + curr_pose_in_map_frame.header.stamp = parent_node->now(); + curr_pose_in_map_frame.pose.position.x = response->map_point.x; + curr_pose_in_map_frame.pose.position.y = response->map_point.y; + curr_pose_in_map_frame.pose.position.z = response->map_point.z; + + geometry_msgs::msg::Quaternion utm_orientation = curr_gps_pose.orientation; geometry_msgs::msg::PoseStamped temporary_pose; temporary_pose.pose.orientation = utm_orientation; temporary_pose.header.frame_id = "utm"; nav_2d_utils::transformPose( tf_buffer_, "map", temporary_pose, temporary_pose, transform_tolerance); - curr_waypoint_in_map_frame.pose.orientation = temporary_pose.pose.orientation; + curr_pose_in_map_frame.pose.orientation = temporary_pose.pose.orientation; - poses_in_map_frame_vector.push_back(curr_waypoint_in_map_frame); + poses_in_map_frame_vector.push_back(curr_pose_in_map_frame); } } RCLCPP_INFO( From 063b8e76fa3a1c500ccc97990cebb8fc87b273d6 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 14 Dec 2020 15:10:49 +0800 Subject: [PATCH 15/70] handle cpplint errors --- nav2_waypoint_follower/src/waypoint_follower.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 573a8854ef..844f7faa3f 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "nav2_waypoint_follower/waypoint_follower.hpp" -#include "nav_2d_utils/tf_help.hpp" #include #include @@ -22,6 +21,8 @@ #include #include +#include "nav_2d_utils/tf_help.hpp" + namespace nav2_waypoint_follower { @@ -388,7 +389,6 @@ WaypointFollower::convertGPSPoses2MapPoses( std::vector poses_in_map_frame_vector; for (auto && curr_gps_pose : gps_poses) { - auto request = std::make_shared(); auto response = std::make_shared(); request->ll_point.latitude = curr_gps_pose.position.latitude; From e544fde99d41ee4b38dc4d8329289c17797ea983 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Tue, 15 Dec 2020 18:13:59 +0800 Subject: [PATCH 16/70] tf_listener needs to be initialized --- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 2 ++ nav2_waypoint_follower/src/waypoint_follower.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index c9a823fdab..e1cf89946d 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -35,6 +35,7 @@ #include "robot_localization/srv/from_ll.hpp" #include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" namespace nav2_waypoint_follower { @@ -198,6 +199,7 @@ class WaypointFollower : public nav2_util::LifecycleNode // tf buffer to get transfroms std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 844f7faa3f..1ba6342ed7 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -89,6 +89,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); // used for transfroming orientation of GPS poses to map frame tf_buffer_ = std::make_shared(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( From 84c7ef0d83c2f492b2d3bc20bf328e93c5f274a2 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Thu, 17 Dec 2020 19:15:00 +0800 Subject: [PATCH 17/70] apply requested changes --- nav2_msgs/msg/OrientedNavSatFix.msg | 2 +- nav2_waypoint_follower/CMakeLists.txt | 6 +-- .../waypoint_follower.hpp | 1 + nav2_waypoint_follower/package.xml | 1 - .../src/waypoint_follower.cpp | 47 ++++++++++--------- 5 files changed, 28 insertions(+), 29 deletions(-) diff --git a/nav2_msgs/msg/OrientedNavSatFix.msg b/nav2_msgs/msg/OrientedNavSatFix.msg index aa5b024ddb..d059e5eae2 100644 --- a/nav2_msgs/msg/OrientedNavSatFix.msg +++ b/nav2_msgs/msg/OrientedNavSatFix.msg @@ -1,2 +1,2 @@ sensor_msgs/NavSatFix position -geometry_msgs/Quaternion orientation \ No newline at end of file +geometry_msgs/Quaternion orientation diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index d3c0afa224..3ca37de2bf 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -2,9 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) # Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +set(CMAKE_CXX_STANDARD 17) # Try for OpenCV 4.X, but settle for whatever is installed find_package(OpenCV 4 QUIET) @@ -23,7 +21,6 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav_2d_utils REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_core REQUIRED) find_package(pluginlib REQUIRED) @@ -54,7 +51,6 @@ set(dependencies nav_msgs nav2_msgs nav2_util - nav_2d_utils tf2_ros nav2_core pluginlib diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index c9a823fdab..315b6190b6 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -35,6 +35,7 @@ #include "robot_localization/srv/from_ll.hpp" #include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" namespace nav2_waypoint_follower { diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 0ed3197310..f16494af58 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -19,7 +19,6 @@ nav_msgs nav2_msgs nav2_util - nav_2d_utils nav2_core tf2_ros robot_localization diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 844f7faa3f..8f0f9f4a3d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -21,8 +21,6 @@ #include #include -#include "nav_2d_utils/tf_help.hpp" - namespace nav2_waypoint_follower { @@ -388,12 +386,14 @@ WaypointFollower::convertGPSPoses2MapPoses( rclcpp::Duration transform_tolerance(0, 500); std::vector poses_in_map_frame_vector; - for (auto && curr_gps_pose : gps_poses) { + auto stamp = parent_node->now(); + + for (auto && curr_oriented_navsat_fix : gps_poses) { auto request = std::make_shared(); auto response = std::make_shared(); - request->ll_point.latitude = curr_gps_pose.position.latitude; - request->ll_point.longitude = curr_gps_pose.position.longitude; - request->ll_point.altitude = curr_gps_pose.position.altitude; + request->ll_point.latitude = curr_oriented_navsat_fix.position.latitude; + request->ll_point.longitude = curr_oriented_navsat_fix.position.longitude; + request->ll_point.altitude = curr_oriented_navsat_fix.position.altitude; fromll_client->wait_for_service((std::chrono::seconds(1))); auto is_conversion_succeeded = fromll_client->invoke( @@ -408,22 +408,25 @@ WaypointFollower::convertGPSPoses2MapPoses( continue; } else { // this poses are assumed to be on global frame (map) - geometry_msgs::msg::PoseStamped curr_pose_in_map_frame; - curr_pose_in_map_frame.header.frame_id = "map"; - curr_pose_in_map_frame.header.stamp = parent_node->now(); - curr_pose_in_map_frame.pose.position.x = response->map_point.x; - curr_pose_in_map_frame.pose.position.y = response->map_point.y; - curr_pose_in_map_frame.pose.position.z = response->map_point.z; - - geometry_msgs::msg::Quaternion utm_orientation = curr_gps_pose.orientation; - geometry_msgs::msg::PoseStamped temporary_pose; - temporary_pose.pose.orientation = utm_orientation; - temporary_pose.header.frame_id = "utm"; - nav_2d_utils::transformPose( - tf_buffer_, "map", temporary_pose, temporary_pose, transform_tolerance); - curr_pose_in_map_frame.pose.orientation = temporary_pose.pose.orientation; - - poses_in_map_frame_vector.push_back(curr_pose_in_map_frame); + geometry_msgs::msg::PoseStamped curr_pose_map_frame; + curr_pose_map_frame.header.frame_id = "map"; + curr_pose_map_frame.header.stamp = stamp; + curr_pose_map_frame.pose.position.x = response->map_point.x; + curr_pose_map_frame.pose.position.y = response->map_point.y; + curr_pose_map_frame.pose.position.z = response->map_point.z; + + geometry_msgs::msg::PoseStamped curr_pose_utm_frame; + curr_pose_utm_frame.pose.orientation = curr_oriented_navsat_fix.orientation; + curr_pose_utm_frame.header.frame_id = "utm"; + try { + tf_buffer_->transform(curr_pose_utm_frame, curr_pose_map_frame, "map"); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + parent_node->get_logger(), + "Exception in itm -> map transform: %s", + ex.what()); + } + poses_in_map_frame_vector.push_back(curr_pose_map_frame); } } RCLCPP_INFO( From aee13ca36fa69aea8e73e3d1f53761fab4cf45a5 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Thu, 17 Dec 2020 19:33:05 +0800 Subject: [PATCH 18/70] apply requested changes 3.0/3.0 --- nav2_waypoint_follower/src/waypoint_follower.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 880bc2a778..722d3622b5 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -397,10 +397,10 @@ WaypointFollower::convertGPSPoses2MapPoses( request->ll_point.altitude = curr_oriented_navsat_fix.position.altitude; fromll_client->wait_for_service((std::chrono::seconds(1))); - auto is_conversion_succeeded = fromll_client->invoke( - request, - response); - if (!is_conversion_succeeded) { + if (!fromll_client->invoke( + request, + response);) + { RCLCPP_ERROR( parent_node->get_logger(), "fromLL service of robot_localization could not convert %i th GPS waypoint to" @@ -420,7 +420,9 @@ WaypointFollower::convertGPSPoses2MapPoses( curr_pose_utm_frame.pose.orientation = curr_oriented_navsat_fix.orientation; curr_pose_utm_frame.header.frame_id = "utm"; try { - tf_buffer_->transform(curr_pose_utm_frame, curr_pose_map_frame, "map"); + tf_buffer_->transform( + curr_pose_utm_frame, curr_pose_map_frame, "map", + tf2::durationFromSec(1.0)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( parent_node->get_logger(), From 4f831674ee8278fb8b6168af0ecdcf6704cc8da4 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Thu, 17 Dec 2020 19:35:05 +0800 Subject: [PATCH 19/70] fix misplaced ";" --- nav2_waypoint_follower/src/waypoint_follower.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 722d3622b5..86ab6ffa84 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -399,7 +399,7 @@ WaypointFollower::convertGPSPoses2MapPoses( fromll_client->wait_for_service((std::chrono::seconds(1))); if (!fromll_client->invoke( request, - response);) + response)) { RCLCPP_ERROR( parent_node->get_logger(), From 6e322eab127aa45f0af3dc660d43918dbae520a8 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Fri, 18 Dec 2020 09:41:56 +0800 Subject: [PATCH 20/70] use run time param for gps transform timeout --- doc/parameters/param_list.md | 1 + nav2_bringup/bringup/params/nav2_params.yaml | 1 + .../nav2_waypoint_follower/waypoint_follower.hpp | 1 + nav2_waypoint_follower/src/waypoint_follower.cpp | 14 ++++++++------ 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 5b67b12d0d..5ee79b7fb9 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -534,6 +534,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | +| gps_waypoint_transform_timeout | 0.1 | Amount of time in seconds to wait before a timeout exception is thrown while transforming a GPS waypoint form `utm` frame to `map ` frame. | | waypoint_task_executor_plugin | `waypoint_task_executor` | Name of plugin to be loaded for executing waypoint tasks.| ## WaitAtWaypoint plugin diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index f10edb7729..c62ad7e39e 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -292,6 +292,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + gps_waypoint_transform_timeout: 0.1 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 279f53e8d1..53902392b6 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -189,6 +189,7 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; std::unique_ptr> from_ll_to_map_client_; + std::shared_ptr gps_waypoint_transform_timeout_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 86ab6ffa84..8f66fc7fa2 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -33,6 +33,8 @@ WaypointFollower::WaypointFollower() declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); + declare_parameter("gps_waypoint_transform_timeout", 0.1); + nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), rclcpp::ParameterValue(std::string("wait_at_waypoint"))); @@ -55,6 +57,10 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); + gps_waypoint_transform_timeout_ = + std::make_shared( + rclcpp::Duration::from_seconds( + get_parameter("gps_waypoint_transform_timeout").as_double())); std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -384,7 +390,6 @@ WaypointFollower::convertGPSPoses2MapPoses( const std::unique_ptr> & fromll_client) { RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); - rclcpp::Duration transform_tolerance(0, 500); std::vector poses_in_map_frame_vector; auto stamp = parent_node->now(); @@ -397,10 +402,7 @@ WaypointFollower::convertGPSPoses2MapPoses( request->ll_point.altitude = curr_oriented_navsat_fix.position.altitude; fromll_client->wait_for_service((std::chrono::seconds(1))); - if (!fromll_client->invoke( - request, - response)) - { + if (!fromll_client->invoke(request, response)) { RCLCPP_ERROR( parent_node->get_logger(), "fromLL service of robot_localization could not convert %i th GPS waypoint to" @@ -422,7 +424,7 @@ WaypointFollower::convertGPSPoses2MapPoses( try { tf_buffer_->transform( curr_pose_utm_frame, curr_pose_map_frame, "map", - tf2::durationFromSec(1.0)); + tf2::durationFromSec(gps_waypoint_transform_timeout_->seconds())); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( parent_node->get_logger(), From 81c7c6496beb0335088b0f228751b36b6012e2c6 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Fri, 18 Dec 2020 10:02:41 +0800 Subject: [PATCH 21/70] change timeout var name --- doc/parameters/param_list.md | 2 +- nav2_bringup/bringup/params/nav2_params.yaml | 2 +- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 2 +- nav2_waypoint_follower/src/waypoint_follower.cpp | 9 +++------ 4 files changed, 6 insertions(+), 9 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 5ee79b7fb9..0327251006 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -534,7 +534,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | -| gps_waypoint_transform_timeout | 0.1 | Amount of time in seconds to wait before a timeout exception is thrown while transforming a GPS waypoint form `utm` frame to `map ` frame. | +| transform_tolerance | 0.1 | Amount of time in seconds to wait before a timeout exception is thrown while transforming a GPS waypoint form `utm` frame to `map ` frame. | | waypoint_task_executor_plugin | `waypoint_task_executor` | Name of plugin to be loaded for executing waypoint tasks.| ## WaitAtWaypoint plugin diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index c62ad7e39e..96194909b3 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -292,7 +292,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - gps_waypoint_transform_timeout: 0.1 + transform_tolerance: 0.1 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 53902392b6..d5962a99d1 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -189,7 +189,7 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; std::unique_ptr> from_ll_to_map_client_; - std::shared_ptr gps_waypoint_transform_timeout_; + double transform_tolerance_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 8f66fc7fa2..8590c31e87 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -33,7 +33,7 @@ WaypointFollower::WaypointFollower() declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); - declare_parameter("gps_waypoint_transform_timeout", 0.1); + declare_parameter("transform_tolerance", 0.1); nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), @@ -57,10 +57,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); - gps_waypoint_transform_timeout_ = - std::make_shared( - rclcpp::Duration::from_seconds( - get_parameter("gps_waypoint_transform_timeout").as_double())); + transform_tolerance_ = get_parameter("transform_tolerance").as_double(); std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -424,7 +421,7 @@ WaypointFollower::convertGPSPoses2MapPoses( try { tf_buffer_->transform( curr_pose_utm_frame, curr_pose_map_frame, "map", - tf2::durationFromSec(gps_waypoint_transform_timeout_->seconds())); + tf2::durationFromSec(transform_tolerance_)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( parent_node->get_logger(), From 7c695202c3efaf7d9e79ed49582c52944ab002a7 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 21 Dec 2020 09:52:28 +0800 Subject: [PATCH 22/70] make use of stop_on_failure for GPS too --- nav2_waypoint_follower/src/waypoint_follower.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 8590c31e87..30334eaf5c 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -405,6 +405,14 @@ WaypointFollower::convertGPSPoses2MapPoses( "fromLL service of robot_localization could not convert %i th GPS waypoint to" "Map frame, going to skip this point!" "Make sure you have run navsat_transform_node of robot_localization"); + if (stop_on_failure_) { + RCLCPP_ERROR( + parent_node->get_logger(), + "Conversion of %i th GPS waypoint to" + "Map frame failed and stop_on_failure is set to true" + "Not going to execute any of waypoints, exiting with failure!"); + return std::vector(); + } continue; } else { // this poses are assumed to be on global frame (map) From 7889ae3fb3e4bca4e312663b3b8023f611291683 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 23 Dec 2020 19:45:03 +0800 Subject: [PATCH 23/70] passing emptywaypont vectors are seen as failure --- nav2_waypoint_follower/src/waypoint_follower.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 30334eaf5c..94d6c181ea 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -199,8 +199,12 @@ void WaypointFollower::followWaypointsLogic( get_logger(), "Received follow waypoint request with %i waypoints.", static_cast(poses.size())); - if (poses.size() == 0) { - action_server->succeeded_current(result); + if (poses.empty()) { + RCLCPP_ERROR( + get_logger(), + "Empty vector of Waypoints passed to waypoint following logic. " + "Nothing to execute, returning with failure!"); + action_server->terminate_current(result); return; } @@ -224,6 +228,14 @@ void WaypointFollower::followWaypointsLogic( RCLCPP_INFO(get_logger(), "Preempting the goal pose."); goal = action_server->accept_pending_goal(); poses = getLatestGoalPoses(action_server); + if (poses.empty()) { + RCLCPP_ERROR( + get_logger(), + "Empty vector of Waypoints passed to waypoint following logic. " + "Nothing to execute, returning with failure!"); + action_server->terminate_current(result); + return; + } goal_index = 0; new_goal = true; } From c6613a88ba87b831b7da42ceacf654f808888edf Mon Sep 17 00:00:00 2001 From: jediofgever Date: Fri, 8 Jan 2021 12:19:45 +0800 Subject: [PATCH 24/70] update warning for empty requests --- nav2_waypoint_follower/src/waypoint_follower.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 94d6c181ea..ba020ff374 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -202,8 +202,9 @@ void WaypointFollower::followWaypointsLogic( if (poses.empty()) { RCLCPP_ERROR( get_logger(), - "Empty vector of Waypoints passed to waypoint following logic. " - "Nothing to execute, returning with failure!"); + "Empty vector of waypoints passed to waypoint following " + "action potentially due to conversation failure or empty request." + ); action_server->terminate_current(result); return; } From d7e2ad41cb2e92e09d6e85bf2e1baa2c9001cee8 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Sat, 23 Jan 2021 14:21:33 +0800 Subject: [PATCH 25/70] consider utm -> map yaw offset --- .../src/waypoint_follower.cpp | 46 ++++++++++++------- 1 file changed, 29 insertions(+), 17 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index ba020ff374..575ed6f315 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -320,7 +320,7 @@ void WaypointFollower::followWaypointsLogic( if (goal_index >= poses.size()) { RCLCPP_INFO( get_logger(), "Completed all %i waypoints requested.", - poses.size()); + static_cast(poses.size())); result->missed_waypoints = failed_ids_; action_server->succeeded_current(result); failed_ids_.clear(); @@ -428,33 +428,45 @@ WaypointFollower::convertGPSPoses2MapPoses( } continue; } else { - // this poses are assumed to be on global frame (map) - geometry_msgs::msg::PoseStamped curr_pose_map_frame; - curr_pose_map_frame.header.frame_id = "map"; - curr_pose_map_frame.header.stamp = stamp; - curr_pose_map_frame.pose.position.x = response->map_point.x; - curr_pose_map_frame.pose.position.y = response->map_point.y; - curr_pose_map_frame.pose.position.z = response->map_point.z; - - geometry_msgs::msg::PoseStamped curr_pose_utm_frame; - curr_pose_utm_frame.pose.orientation = curr_oriented_navsat_fix.orientation; - curr_pose_utm_frame.header.frame_id = "utm"; + // fromll_client converted the point from LL to Map frames + // but it actually did not consider the possible yaw offset between utm and map frames ; + // "https://github.com/cra-ros-pkg/robot_localization/blob/ + // 79162b2ac53a112c51d23859c499e8438cf9938e/src/navsat_transform.cpp#L394" + // see above link on how they set rotation between UTM and + // Map to Identity , where actually it might not be + geometry_msgs::msg::TransformStamped utm_to_map_transform; try { - tf_buffer_->transform( - curr_pose_utm_frame, curr_pose_map_frame, "map", - tf2::durationFromSec(transform_tolerance_)); + utm_to_map_transform = tf_buffer_->lookupTransform("utm", "map", tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( parent_node->get_logger(), - "Exception in itm -> map transform: %s", + "Exception in getting utm -> map transform: %s", ex.what()); } + tf2::Quaternion utm_to_map_quat; + tf2::fromMsg(utm_to_map_transform.transform.rotation, utm_to_map_quat); + double roll, pitch, yaw; + tf2::Matrix3x3 m(utm_to_map_quat); m.getRPY(roll, pitch, yaw); + // we need to consider the possible yaw_offset between utm and map here, + // rotate x , y with amount of yaw + double x = response->map_point.x; + double y = response->map_point.y; + double x_dot = x * std::cos(yaw) - y * std::sin(yaw); + double y_dot = x * std::sin(yaw) + y * std::cos(yaw); + geometry_msgs::msg::PoseStamped curr_pose_map_frame; + curr_pose_map_frame.header.frame_id = "map"; + curr_pose_map_frame.header.stamp = stamp; + curr_pose_map_frame.pose.position.x = x_dot; + curr_pose_map_frame.pose.position.y = y_dot; + curr_pose_map_frame.pose.position.z = response->map_point.z; + curr_pose_map_frame.pose.orientation = curr_oriented_navsat_fix.orientation; poses_in_map_frame_vector.push_back(curr_pose_map_frame); } } RCLCPP_INFO( parent_node->get_logger(), - "Converted all %i GPS waypoint to Map frame", poses_in_map_frame_vector.size()); + "Converted all %i GPS waypoint to Map frame", + static_cast(poses_in_map_frame_vector.size())); return poses_in_map_frame_vector; } From d6527d20150b6ebdd695f24db36222f5a39f82b9 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Sat, 23 Jan 2021 14:35:33 +0800 Subject: [PATCH 26/70] fix missed RCLCPP info --- nav2_waypoint_follower/src/waypoint_follower.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 575ed6f315..a5ee8b8b7b 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -403,7 +403,7 @@ WaypointFollower::convertGPSPoses2MapPoses( std::vector poses_in_map_frame_vector; auto stamp = parent_node->now(); - + int waypoint_index = 0; for (auto && curr_oriented_navsat_fix : gps_poses) { auto request = std::make_shared(); auto response = std::make_shared(); @@ -417,13 +417,13 @@ WaypointFollower::convertGPSPoses2MapPoses( parent_node->get_logger(), "fromLL service of robot_localization could not convert %i th GPS waypoint to" "Map frame, going to skip this point!" - "Make sure you have run navsat_transform_node of robot_localization"); + "Make sure you have run navsat_transform_node of robot_localization", waypoint_index); if (stop_on_failure_) { RCLCPP_ERROR( parent_node->get_logger(), "Conversion of %i th GPS waypoint to" "Map frame failed and stop_on_failure is set to true" - "Not going to execute any of waypoints, exiting with failure!"); + "Not going to execute any of waypoints, exiting with failure!", waypoint_index); return std::vector(); } continue; @@ -462,6 +462,7 @@ WaypointFollower::convertGPSPoses2MapPoses( curr_pose_map_frame.pose.orientation = curr_oriented_navsat_fix.orientation; poses_in_map_frame_vector.push_back(curr_pose_map_frame); } + waypoint_index++; } RCLCPP_INFO( parent_node->get_logger(), From f99ebff38cfbed01428f76fe8e5cfe8a2bde5491 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Sat, 23 Jan 2021 15:49:09 +0800 Subject: [PATCH 27/70] reorrect action;s name --- nav2_waypoint_follower/README.md | 2 +- nav2_waypoint_follower/src/waypoint_follower.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 97b6666805..aeb8351125 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -40,7 +40,7 @@ For instance, ```cpp using ClientT = nav2_msgs::action::FollowGPSWaypoints; rclcpp_action::Client::SharedPtr gps_waypoint_follower_action_client_; -gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "FollowGPSWaypoints"); +gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "follow_gps_waypoints"); ``` All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a5ee8b8b7b..557ffe466d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -75,7 +75,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "FollowWaypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); + "follow_waypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); from_ll_to_map_client_ = std::make_unique< nav2_util::ServiceClient>( @@ -87,7 +87,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "FollowGPSWaypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); + "follow_gps_waypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); // used for transfroming orientation of GPS poses to map frame tf_buffer_ = std::make_shared(node->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); From 5fcfb835d5935da5e605d2ee3f3dc7e955343739 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Fri, 19 Mar 2021 18:43:23 +0800 Subject: [PATCH 28/70] waypoint stamps need to be updated --- nav2_waypoint_follower/src/waypoint_follower.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 557ffe466d..c791e26d9c 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -246,6 +246,7 @@ void WaypointFollower::followWaypointsLogic( new_goal = false; ClientT::Goal client_goal; client_goal.pose = poses[goal_index]; + client_goal.pose.header.stamp = this->now(); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback = @@ -316,6 +317,7 @@ void WaypointFollower::followWaypointsLogic( { // Update server state goal_index++; + poses[goal_index].header.stamp = this->now(); new_goal = true; if (goal_index >= poses.size()) { RCLCPP_INFO( @@ -402,7 +404,6 @@ WaypointFollower::convertGPSPoses2MapPoses( RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); std::vector poses_in_map_frame_vector; - auto stamp = parent_node->now(); int waypoint_index = 0; for (auto && curr_oriented_navsat_fix : gps_poses) { auto request = std::make_shared(); @@ -455,7 +456,7 @@ WaypointFollower::convertGPSPoses2MapPoses( double y_dot = x * std::sin(yaw) + y * std::cos(yaw); geometry_msgs::msg::PoseStamped curr_pose_map_frame; curr_pose_map_frame.header.frame_id = "map"; - curr_pose_map_frame.header.stamp = stamp; + curr_pose_map_frame.header.stamp = parent_node->now(); curr_pose_map_frame.pose.position.x = x_dot; curr_pose_map_frame.pose.position.y = y_dot; curr_pose_map_frame.pose.position.z = response->map_point.z; From 3e7af6bc7f3c2750560eeef6369073649a2f70f1 Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 10 Feb 2022 19:05:48 +0000 Subject: [PATCH 29/70] Fix segmentation fault on waypoint follower --- nav2_waypoint_follower/src/waypoint_follower.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 065c566eee..acb242ae5c 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -67,6 +67,14 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) false); callback_group_executor_.add_callback_group(callback_group_, get_node_base_interface()); + std::vector new_args = rclcpp::NodeOptions().arguments(); + new_args.push_back("--ros-args"); + new_args.push_back("-r"); + new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); + new_args.push_back("--"); + client_node_ = std::make_shared( + "_", "", rclcpp::NodeOptions().arguments(new_args)); + nav_to_pose_client_ = rclcpp_action::create_client( get_node_base_interface(), get_node_graph_interface(), From bc8776338d8e8d2ec5e8f11ec3de288fef6f86fa Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 10 Feb 2022 23:11:51 +0000 Subject: [PATCH 30/70] Parametric frames and matrix multiplications --- .../waypoint_follower.hpp | 5 ++- .../src/waypoint_follower.cpp | 32 +++++++++++-------- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 09f44a3205..232fb7539d 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -30,13 +30,14 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_util/simple_action_server.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/string_utils.hpp" #include "nav2_msgs/action/follow_gps_waypoints.hpp" #include "nav2_util/service_client.hpp" #include "nav2_core/waypoint_task_executor.hpp" #include "robot_localization/srv/from_ll.hpp" #include "tf2_ros/buffer.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_listener.h" namespace nav2_waypoint_follower @@ -181,6 +182,8 @@ class WaypointFollower : public nav2_util::LifecycleNode std::vector failed_ids_; int loop_rate_; bool stop_on_failure_; + std::string global_frame_id_{"map"}; + std::string utm_frame_id_{"utm"}; /** * @brief Callback executed when a parameter change is detected diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index acb242ae5c..418f6269ab 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -37,6 +37,8 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); declare_parameter("transform_tolerance", 0.1); + declare_parameter("global_frame_id", global_frame_id_); + declare_parameter("utm_frame_id", utm_frame_id_); nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), @@ -44,6 +46,9 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) nav2_util::declare_parameter_if_not_declared( this, std::string("wait_at_waypoint.plugin"), rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); + + global_frame_id_ = nav2_util::strip_leading_slash(global_frame_id_); + utm_frame_id_ = nav2_util::strip_leading_slash(utm_frame_id_); } WaypointFollower::~WaypointFollower() @@ -437,6 +442,8 @@ WaypointFollower::dynamicParametersCallback(std::vector param result.successful = true; return result; +} + std::vector WaypointFollower::convertGPSPoses2MapPoses( const std::vector & gps_poses, @@ -479,28 +486,25 @@ WaypointFollower::convertGPSPoses2MapPoses( // Map to Identity , where actually it might not be geometry_msgs::msg::TransformStamped utm_to_map_transform; try { - utm_to_map_transform = tf_buffer_->lookupTransform("utm", "map", tf2::TimePointZero); + utm_to_map_transform = tf_buffer_->lookupTransform(utm_frame_id_, global_frame_id_, tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( parent_node->get_logger(), - "Exception in getting utm -> map transform: %s", - ex.what()); + "Exception in getting %s -> %s transform: %s", + utm_frame_id_.c_str(), global_frame_id_.c_str(), ex.what()); } - tf2::Quaternion utm_to_map_quat; - tf2::fromMsg(utm_to_map_transform.transform.rotation, utm_to_map_quat); - double roll, pitch, yaw; - tf2::Matrix3x3 m(utm_to_map_quat); m.getRPY(roll, pitch, yaw); // we need to consider the possible yaw_offset between utm and map here, // rotate x , y with amount of yaw - double x = response->map_point.x; - double y = response->map_point.y; - double x_dot = x * std::cos(yaw) - y * std::sin(yaw); - double y_dot = x * std::sin(yaw) + y * std::cos(yaw); + tf2::Quaternion utm_to_map_quat; + tf2::fromMsg(utm_to_map_transform.transform.rotation, utm_to_map_quat); + const tf2::Matrix3x3 m(utm_to_map_quat); + const tf2::Vector3 point({response->map_point.x, response->map_point.y, response->map_point.z}); + auto corrected_point = m*point; geometry_msgs::msg::PoseStamped curr_pose_map_frame; - curr_pose_map_frame.header.frame_id = "map"; + curr_pose_map_frame.header.frame_id = global_frame_id_; curr_pose_map_frame.header.stamp = parent_node->now(); - curr_pose_map_frame.pose.position.x = x_dot; - curr_pose_map_frame.pose.position.y = y_dot; + curr_pose_map_frame.pose.position.x = corrected_point[0]; + curr_pose_map_frame.pose.position.y = corrected_point[1]; curr_pose_map_frame.pose.position.z = response->map_point.z; curr_pose_map_frame.pose.orientation = curr_oriented_navsat_fix.orientation; poses_in_map_frame_vector.push_back(curr_pose_map_frame); From cfb6f13c05b2011057feac5d97ced80afd866b35 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 11 Feb 2022 14:25:25 +0000 Subject: [PATCH 31/70] Replace oriented navsatfix for geographic_msgs/geopose --- nav2_msgs/CMakeLists.txt | 5 ++--- nav2_msgs/action/FollowGPSWaypoints.action | 3 ++- nav2_msgs/msg/OrientedNavSatFix.msg | 2 -- nav2_msgs/package.xml | 2 +- nav2_waypoint_follower/CMakeLists.txt | 1 + .../nav2_waypoint_follower/waypoint_follower.hpp | 3 ++- nav2_waypoint_follower/package.xml | 3 ++- nav2_waypoint_follower/src/waypoint_follower.cpp | 12 ++++++------ 8 files changed, 16 insertions(+), 15 deletions(-) delete mode 100644 nav2_msgs/msg/OrientedNavSatFix.msg diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 0ff1d5618f..e198d09148 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) -find_package(sensor_msgs) +find_package(geographic_msgs) find_package(action_msgs REQUIRED) nav2_package() @@ -23,7 +23,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" - "msg/OrientedNavSatFix.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" @@ -44,7 +43,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/DummyRecovery.action" "action/FollowWaypoints.action" "action/FollowGPSWaypoints.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs sensor_msgs + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action index 45a24ae7f3..f296b9348b 100644 --- a/nav2_msgs/action/FollowGPSWaypoints.action +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -1,5 +1,6 @@ #goal definition -nav2_msgs/OrientedNavSatFix[] gps_poses +# nav2_msgs/OrientedNavSatFix[] gps_poses +geographic_msgs/GeoPose[] gps_poses --- #result definition int32[] missed_waypoints diff --git a/nav2_msgs/msg/OrientedNavSatFix.msg b/nav2_msgs/msg/OrientedNavSatFix.msg deleted file mode 100644 index d059e5eae2..0000000000 --- a/nav2_msgs/msg/OrientedNavSatFix.msg +++ /dev/null @@ -1,2 +0,0 @@ -sensor_msgs/NavSatFix position -geometry_msgs/Quaternion orientation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index d209e2867f..245a6e22f2 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -19,7 +19,7 @@ geometry_msgs action_msgs nav_msgs - sensor_msgs + geographic_msgs rosidl_interface_packages diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index b51a9db0c2..d09de8fc3b 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED) find_package(nav2_core REQUIRED) find_package(pluginlib REQUIRED) find_package(robot_localization REQUIRED) +find_package(geographic_msgs REQUIRED) nav2_package() diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 232fb7539d..d010a3a81d 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -24,6 +24,7 @@ #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_msgs/msg/oriented_nav_sat_fix.hpp" +#include "geographic_msgs/msg/geo_pose.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" @@ -169,7 +170,7 @@ class WaypointFollower : public nav2_util::LifecycleNode * @return std::vector */ std::vector convertGPSPoses2MapPoses( - const std::vector & gps_poses, + const std::vector & gps_poses, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const std::unique_ptr> & fromll_client); diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index f9e2f71db8..b6334b2d1a 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -21,7 +21,8 @@ nav2_util nav2_core tf2_ros - robot_localization + robot_localization + geographic_msgs ament_lint_common ament_lint_auto diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 418f6269ab..3cdec5152a 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -446,7 +446,7 @@ WaypointFollower::dynamicParametersCallback(std::vector param std::vector WaypointFollower::convertGPSPoses2MapPoses( - const std::vector & gps_poses, + const std::vector & gps_poses, const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const std::unique_ptr> & fromll_client) { @@ -454,12 +454,12 @@ WaypointFollower::convertGPSPoses2MapPoses( std::vector poses_in_map_frame_vector; int waypoint_index = 0; - for (auto && curr_oriented_navsat_fix : gps_poses) { + for (auto && curr_geopose : gps_poses) { auto request = std::make_shared(); auto response = std::make_shared(); - request->ll_point.latitude = curr_oriented_navsat_fix.position.latitude; - request->ll_point.longitude = curr_oriented_navsat_fix.position.longitude; - request->ll_point.altitude = curr_oriented_navsat_fix.position.altitude; + request->ll_point.latitude = curr_geopose.position.latitude; + request->ll_point.longitude = curr_geopose.position.longitude; + request->ll_point.altitude = curr_geopose.position.altitude; fromll_client->wait_for_service((std::chrono::seconds(1))); if (!fromll_client->invoke(request, response)) { @@ -506,7 +506,7 @@ WaypointFollower::convertGPSPoses2MapPoses( curr_pose_map_frame.pose.position.x = corrected_point[0]; curr_pose_map_frame.pose.position.y = corrected_point[1]; curr_pose_map_frame.pose.position.z = response->map_point.z; - curr_pose_map_frame.pose.orientation = curr_oriented_navsat_fix.orientation; + curr_pose_map_frame.pose.orientation = curr_geopose.orientation; poses_in_map_frame_vector.push_back(curr_pose_map_frame); } waypoint_index++; From cafef3946dccca3de13b59203dd2230e37428854 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 11 Feb 2022 15:22:02 +0000 Subject: [PATCH 32/70] Remove deprecated oriented navsatfix message --- nav2_msgs/action/FollowGPSWaypoints.action | 2 +- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action index f296b9348b..0fd681e7be 100644 --- a/nav2_msgs/action/FollowGPSWaypoints.action +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -1,5 +1,5 @@ #goal definition -# nav2_msgs/OrientedNavSatFix[] gps_poses +# geographic_msgs/GeoPose[] gps_poses geographic_msgs/GeoPose[] gps_poses --- #result definition diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index d010a3a81d..86b2b9489d 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -23,7 +23,6 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_msgs/msg/oriented_nav_sat_fix.hpp" #include "geographic_msgs/msg/geo_pose.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" From 4b3f774f1f780c26966246f57a29125cc686396a Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 11 Feb 2022 15:27:26 +0000 Subject: [PATCH 33/70] Update branch name on robot_localization dependency --- tools/underlay.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 7a2fd5fcd0..2a4890cc0c 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -38,5 +38,5 @@ repositories: robot_localization/robot_localization: type: git url: https://github.com/cra-ros-pkg/robot_localization.git - version: galactic + version: ros2 From 534f01a851b7a6fcde61d6b3d7ffb53141cf2102 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 11 Feb 2022 16:13:29 +0000 Subject: [PATCH 34/70] Fix parametric frames logic --- .../src/waypoint_follower.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 3cdec5152a..fc09137afd 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -46,9 +46,6 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) nav2_util::declare_parameter_if_not_declared( this, std::string("wait_at_waypoint.plugin"), rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); - - global_frame_id_ = nav2_util::strip_leading_slash(global_frame_id_); - utm_frame_id_ = nav2_util::strip_leading_slash(utm_frame_id_); } WaypointFollower::~WaypointFollower() @@ -66,6 +63,11 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); transform_tolerance_ = get_parameter("transform_tolerance").as_double(); + global_frame_id_ = get_parameter("global_frame_id").as_string(); + utm_frame_id_ = get_parameter("utm_frame_id").as_string(); + + global_frame_id_ = nav2_util::strip_leading_slash(global_frame_id_); + utm_frame_id_ = nav2_util::strip_leading_slash(utm_frame_id_); callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, @@ -450,7 +452,8 @@ WaypointFollower::convertGPSPoses2MapPoses( const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, const std::unique_ptr> & fromll_client) { - RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to Map Frame.."); + RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to %s Frame..", + global_frame_id_.c_str()); std::vector poses_in_map_frame_vector; int waypoint_index = 0; @@ -466,14 +469,14 @@ WaypointFollower::convertGPSPoses2MapPoses( RCLCPP_ERROR( parent_node->get_logger(), "fromLL service of robot_localization could not convert %i th GPS waypoint to" - "Map frame, going to skip this point!" - "Make sure you have run navsat_transform_node of robot_localization", waypoint_index); + "%s frame, going to skip this point!" + "Make sure you have run navsat_transform_node of robot_localization", waypoint_index, global_frame_id_.c_str()); if (stop_on_failure_) { RCLCPP_ERROR( parent_node->get_logger(), "Conversion of %i th GPS waypoint to" - "Map frame failed and stop_on_failure is set to true" - "Not going to execute any of waypoints, exiting with failure!", waypoint_index); + "%s frame failed and stop_on_failure is set to true" + "Not going to execute any of waypoints, exiting with failure!", waypoint_index, global_frame_id_.c_str()); return std::vector(); } continue; @@ -513,8 +516,8 @@ WaypointFollower::convertGPSPoses2MapPoses( } RCLCPP_INFO( parent_node->get_logger(), - "Converted all %i GPS waypoint to Map frame", - static_cast(poses_in_map_frame_vector.size())); + "Converted all %i GPS waypoint to %s frame", + static_cast(poses_in_map_frame_vector.size()), global_frame_id_.c_str()); return poses_in_map_frame_vector; } From f4466b6ffd709d8981eaf3ec7bdb19cc07e49409 Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 14 Feb 2022 20:36:52 +0000 Subject: [PATCH 35/70] Rename functions and adress comments --- nav2_msgs/action/FollowGPSWaypoints.action | 1 - .../waypoint_follower.hpp | 30 +++---- .../src/waypoint_follower.cpp | 36 ++++----- tools/underlay.repos | 80 +++++++++---------- 4 files changed, 68 insertions(+), 79 deletions(-) diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action index 0fd681e7be..d4e39ca9a0 100644 --- a/nav2_msgs/action/FollowGPSWaypoints.action +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -1,5 +1,4 @@ #goal definition -# geographic_msgs/GeoPose[] gps_poses geographic_msgs/GeoPose[] gps_poses --- #result definition diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 86b2b9489d..bcb40e0f6a 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -125,11 +125,8 @@ class WaypointFollower : public nav2_util::LifecycleNode * @param feedback * @param result */ - template - void followWaypointsLogic( - const T & action_server, - const V & feedback, - const Z & result); + template + void followWaypointsHandler(const T& action_server, const V& feedback, const Z& result); /** * @brief Action server callbacks @@ -159,20 +156,15 @@ class WaypointFollower : public nav2_util::LifecycleNode template void goalResponseCallback(const T & goal); -/** - * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. - * Constructs a vector of stamped poses in map frame and returns them. - * - * @param gps_poses - * @param parent_node - * @param fromll_client - * @return std::vector - */ - std::vector convertGPSPoses2MapPoses( - const std::vector & gps_poses, - const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - const - std::unique_ptr> & fromll_client); + /** + * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. + * Constructs a vector of stamped poses in map frame and returns them. + * + * @param gps_poses + * @return std::vector + */ + std::vector convertGPSPosesToMapPoses( + const std::vector& gps_poses); template std::vector getLatestGoalPoses(const T & action_server); diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index fc09137afd..f5ba9d7f38 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -107,6 +107,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_logging_interface(), get_node_waitables_interface(), "follow_gps_waypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); + // used for transfroming orientation of GPS poses to map frame tf_buffer_ = std::make_shared(node->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -197,15 +198,14 @@ std::vector WaypointFollower::getLatestGoalPose poses = action_server->get_current_goal()->poses; } else { // If GPS waypoint following callback was called, we build here - poses = convertGPSPoses2MapPoses( - action_server->get_current_goal()->gps_poses, shared_from_this(), - from_ll_to_map_client_); + poses = convertGPSPosesToMapPoses( + action_server->get_current_goal()->gps_poses); } return poses; } template -void WaypointFollower::followWaypointsLogic( +void WaypointFollower::followWaypointsHandler( const T & action_server, const V & feedback, const Z & result) @@ -370,7 +370,7 @@ void WaypointFollower::followWaypointsCallback() auto feedback = std::make_shared(); auto result = std::make_shared(); - followWaypointsLogic, + followWaypointsHandler, ActionT::Feedback::SharedPtr, ActionT::Result::SharedPtr>( action_server_, @@ -382,7 +382,7 @@ void WaypointFollower::followGPSWaypointsCallback() auto feedback = std::make_shared(); auto result = std::make_shared(); - followWaypointsLogic, + followWaypointsHandler, ActionTGPS::Feedback::SharedPtr, ActionTGPS::Result::SharedPtr>( gps_action_server_, @@ -447,12 +447,10 @@ WaypointFollower::dynamicParametersCallback(std::vector param } std::vector -WaypointFollower::convertGPSPoses2MapPoses( - const std::vector & gps_poses, - const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, - const std::unique_ptr> & fromll_client) +WaypointFollower::convertGPSPosesToMapPoses( + const std::vector & gps_poses) { - RCLCPP_INFO(parent_node->get_logger(), "Converting GPS waypoints to %s Frame..", + RCLCPP_INFO(this->get_logger(), "Converting GPS waypoints to %s Frame..", global_frame_id_.c_str()); std::vector poses_in_map_frame_vector; @@ -464,16 +462,16 @@ WaypointFollower::convertGPSPoses2MapPoses( request->ll_point.longitude = curr_geopose.position.longitude; request->ll_point.altitude = curr_geopose.position.altitude; - fromll_client->wait_for_service((std::chrono::seconds(1))); - if (!fromll_client->invoke(request, response)) { + from_ll_to_map_client_->wait_for_service((std::chrono::seconds(1))); + if (!from_ll_to_map_client_->invoke(request, response)) { RCLCPP_ERROR( - parent_node->get_logger(), + this->get_logger(), "fromLL service of robot_localization could not convert %i th GPS waypoint to" "%s frame, going to skip this point!" "Make sure you have run navsat_transform_node of robot_localization", waypoint_index, global_frame_id_.c_str()); if (stop_on_failure_) { RCLCPP_ERROR( - parent_node->get_logger(), + this->get_logger(), "Conversion of %i th GPS waypoint to" "%s frame failed and stop_on_failure is set to true" "Not going to execute any of waypoints, exiting with failure!", waypoint_index, global_frame_id_.c_str()); @@ -481,7 +479,7 @@ WaypointFollower::convertGPSPoses2MapPoses( } continue; } else { - // fromll_client converted the point from LL to Map frames + // from_ll_to_map_client_ converted the point from LL to Map frames // but it actually did not consider the possible yaw offset between utm and map frames ; // "https://github.com/cra-ros-pkg/robot_localization/blob/ // 79162b2ac53a112c51d23859c499e8438cf9938e/src/navsat_transform.cpp#L394" @@ -492,7 +490,7 @@ WaypointFollower::convertGPSPoses2MapPoses( utm_to_map_transform = tf_buffer_->lookupTransform(utm_frame_id_, global_frame_id_, tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( - parent_node->get_logger(), + this->get_logger(), "Exception in getting %s -> %s transform: %s", utm_frame_id_.c_str(), global_frame_id_.c_str(), ex.what()); } @@ -505,7 +503,7 @@ WaypointFollower::convertGPSPoses2MapPoses( auto corrected_point = m*point; geometry_msgs::msg::PoseStamped curr_pose_map_frame; curr_pose_map_frame.header.frame_id = global_frame_id_; - curr_pose_map_frame.header.stamp = parent_node->now(); + curr_pose_map_frame.header.stamp = this->now(); curr_pose_map_frame.pose.position.x = corrected_point[0]; curr_pose_map_frame.pose.position.y = corrected_point[1]; curr_pose_map_frame.pose.position.z = response->map_point.z; @@ -515,7 +513,7 @@ WaypointFollower::convertGPSPoses2MapPoses( waypoint_index++; } RCLCPP_INFO( - parent_node->get_logger(), + this->get_logger(), "Converted all %i GPS waypoint to %s frame", static_cast(poses_in_map_frame_vector.size()), global_frame_id_.c_str()); return poses_in_map_frame_vector; diff --git a/tools/underlay.repos b/tools/underlay.repos index 2a4890cc0c..42d4cc93a5 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,42 +1,42 @@ repositories: - BehaviorTree/BehaviorTree.CPP: - type: git - url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: master - ros/angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 - ros-simulation/gazebo_ros_pkgs: - type: git - url: https://github.com/ros-simulation/gazebo_ros_pkgs.git - version: ros2 - ros-perception/image_common: - type: git - url: https://github.com/ros-perception/image_common.git - version: ros2 - ros-perception/vision_opencv: - type: git - url: https://github.com/ros-perception/vision_opencv.git - version: ros2 - ros/bond_core: - type: git - url: https://github.com/ros/bond_core.git - version: ros2 - ros/diagnostics: - type: git - url: https://github.com/ros/diagnostics.git - version: ros2-devel - ros/geographic_info: - type: git - url: https://github.com/ros-geographic-info/geographic_info.git - version: ros2 - ompl/ompl: - type: git - url: https://github.com/ompl/ompl.git - version: 1.5.0 - robot_localization/robot_localization: - type: git - url: https://github.com/cra-ros-pkg/robot_localization.git - version: ros2 +# BehaviorTree/BehaviorTree.CPP: +# type: git +# url: https://github.com/BehaviorTree/BehaviorTree.CPP.git +# version: master +# ros/angles: +# type: git +# url: https://github.com/ros/angles.git +# version: ros2 +# ros-simulation/gazebo_ros_pkgs: +# type: git +# url: https://github.com/ros-simulation/gazebo_ros_pkgs.git +# version: ros2 +# ros-perception/image_common: +# type: git +# url: https://github.com/ros-perception/image_common.git +# version: ros2 +# ros-perception/vision_opencv: +# type: git +# url: https://github.com/ros-perception/vision_opencv.git +# version: ros2 +# ros/bond_core: +# type: git +# url: https://github.com/ros/bond_core.git +# version: ros2 +# ros/diagnostics: +# type: git +# url: https://github.com/ros/diagnostics.git +# version: ros2-devel +# ros/geographic_info: +# type: git +# url: https://github.com/ros-geographic-info/geographic_info.git +# version: ros2 +# ompl/ompl: +# type: git +# url: https://github.com/ompl/ompl.git +# version: 1.5.0 +# robot_localization/robot_localization: +# type: git +# url: https://github.com/cra-ros-pkg/robot_localization.git +# version: ros2 From 9cdefafabe43e4b0e0e433b420ec8fcb3a184298 Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 14 Feb 2022 20:46:02 +0000 Subject: [PATCH 36/70] fix style in underlay.repos --- tools/underlay.repos | 81 ++++++++++++++++++++++---------------------- 1 file changed, 41 insertions(+), 40 deletions(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 42d4cc93a5..ebb2999035 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,42 +1,43 @@ repositories: -# BehaviorTree/BehaviorTree.CPP: -# type: git -# url: https://github.com/BehaviorTree/BehaviorTree.CPP.git -# version: master -# ros/angles: -# type: git -# url: https://github.com/ros/angles.git -# version: ros2 -# ros-simulation/gazebo_ros_pkgs: -# type: git -# url: https://github.com/ros-simulation/gazebo_ros_pkgs.git -# version: ros2 -# ros-perception/image_common: -# type: git -# url: https://github.com/ros-perception/image_common.git -# version: ros2 -# ros-perception/vision_opencv: -# type: git -# url: https://github.com/ros-perception/vision_opencv.git -# version: ros2 -# ros/bond_core: -# type: git -# url: https://github.com/ros/bond_core.git -# version: ros2 -# ros/diagnostics: -# type: git -# url: https://github.com/ros/diagnostics.git -# version: ros2-devel -# ros/geographic_info: -# type: git -# url: https://github.com/ros-geographic-info/geographic_info.git -# version: ros2 -# ompl/ompl: -# type: git -# url: https://github.com/ompl/ompl.git -# version: 1.5.0 -# robot_localization/robot_localization: -# type: git -# url: https://github.com/cra-ros-pkg/robot_localization.git -# version: ros2 +repositories: + # BehaviorTree/BehaviorTree.CPP: + # type: git + # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + # version: master + # ros/angles: + # type: git + # url: https://github.com/ros/angles.git + # version: ros2 + # ros-simulation/gazebo_ros_pkgs: + # type: git + # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git + # version: ros2 + # ros-perception/image_common: + # type: git + # url: https://github.com/ros-perception/image_common.git + # version: ros2 + # ros-perception/vision_opencv: + # type: git + # url: https://github.com/ros-perception/vision_opencv.git + # version: ros2 + # ros/bond_core: + # type: git + # url: https://github.com/ros/bond_core.git + # version: ros2 + # ros/diagnostics: + # type: git + # url: https://github.com/ros/diagnostics.git + # version: ros2-devel + # ros/geographic_info: + # type: git + # url: https://github.com/ros-geographic-info/geographic_info.git + # version: ros2 + # ompl/ompl: + # type: git + # url: https://github.com/ompl/ompl.git + # version: 1.5.0 + # robot_localization/robot_localization: + # type: git + # url: https://github.com/cra-ros-pkg/robot_localization.git + # version: ros2 From 6aee420e6424aa84f5a839dc38b4b3bea68533cd Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 14 Feb 2022 20:47:20 +0000 Subject: [PATCH 37/70] remove duplicate word in underlay.repos --- tools/underlay.repos | 1 - 1 file changed, 1 deletion(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index ebb2999035..c13f78c484 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,4 +1,3 @@ -repositories: repositories: # BehaviorTree/BehaviorTree.CPP: # type: git From c7c8c01eb86ae203c452e20caf6233979c0cb94f Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 14 Feb 2022 20:48:34 +0000 Subject: [PATCH 38/70] update dependency version of ompl --- tools/underlay.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index c13f78c484..4262bdd0fb 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -34,7 +34,7 @@ repositories: # ompl/ompl: # type: git # url: https://github.com/ompl/ompl.git - # version: 1.5.0 + # version: main # robot_localization/robot_localization: # type: git # url: https://github.com/cra-ros-pkg/robot_localization.git From eabbfcf9261f323b80fcd14d4b006c6ba8772d91 Mon Sep 17 00:00:00 2001 From: pepisg Date: Tue, 15 Feb 2022 16:06:37 -0500 Subject: [PATCH 39/70] Template ServiceClient class to accept lifecycle node --- nav2_util/include/nav2_util/service_client.hpp | 9 +++++---- .../nav2_waypoint_follower/waypoint_follower.hpp | 2 +- nav2_waypoint_follower/src/waypoint_follower.cpp | 12 ++---------- tools/underlay.repos | 4 ---- 4 files changed, 8 insertions(+), 19 deletions(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 4af107abf1..168dd6437a 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -26,7 +26,7 @@ namespace nav2_util * @class nav2_util::ServiceClient * @brief A simple wrapper on ROS2 services for invoke() and block-style calling */ -template +template class ServiceClient { public: @@ -37,14 +37,15 @@ class ServiceClient */ explicit ServiceClient( const std::string & service_name, - const rclcpp::Node::SharedPtr & provided_node) + const NodeT & provided_node) : service_name_(service_name), node_(provided_node) { callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - client_ = node_->create_client( + // see: https://stackoverflow.com/a/1682885 + client_ = node_->template create_client( service_name, rmw_qos_profile_services_default, callback_group_); @@ -134,7 +135,7 @@ class ServiceClient protected: std::string service_name_; - rclcpp::Node::SharedPtr node_; + NodeT node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; typename rclcpp::Client::SharedPtr client_; diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index bcb40e0f6a..69f66cac2c 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -197,7 +197,7 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; - std::unique_ptr> from_ll_to_map_client_; + std::unique_ptr>> from_ll_to_map_client_; double transform_tolerance_; // Task Execution At Waypoint Plugin diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index f5ba9d7f38..6a03808859 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -74,14 +74,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) false); callback_group_executor_.add_callback_group(callback_group_, get_node_base_interface()); - std::vector new_args = rclcpp::NodeOptions().arguments(); - new_args.push_back("--ros-args"); - new_args.push_back("-r"); - new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); - new_args.push_back("--"); - client_node_ = std::make_shared( - "_", "", rclcpp::NodeOptions().arguments(new_args)); - nav_to_pose_client_ = rclcpp_action::create_client( get_node_base_interface(), get_node_graph_interface(), @@ -97,9 +89,9 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) "follow_waypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); from_ll_to_map_client_ = std::make_unique< - nav2_util::ServiceClient>( + nav2_util::ServiceClient>>( "/fromLL", - client_node_); + node); gps_action_server_ = std::make_unique( get_node_base_interface(), diff --git a/tools/underlay.repos b/tools/underlay.repos index 4262bdd0fb..07d8160978 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -11,10 +11,6 @@ repositories: # type: git # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git # version: ros2 - # ros-perception/image_common: - # type: git - # url: https://github.com/ros-perception/image_common.git - # version: ros2 # ros-perception/vision_opencv: # type: git # url: https://github.com/ros-perception/vision_opencv.git From af8f4686bed77f6f8edc7e6a1d2514d5206fc7ee Mon Sep 17 00:00:00 2001 From: pepisg Date: Tue, 15 Feb 2022 17:09:11 -0500 Subject: [PATCH 40/70] Remove link to stackoverflow answer --- nav2_util/include/nav2_util/service_client.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 168dd6437a..1699cc6e66 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -44,7 +44,6 @@ class ServiceClient rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - // see: https://stackoverflow.com/a/1682885 client_ = node_->template create_client( service_name, rmw_qos_profile_services_default, From b14fcca70de8118ba501c2450b0950902079346a Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 28 Mar 2022 15:32:00 -0500 Subject: [PATCH 41/70] Remove yaw offset compensation --- .../src/waypoint_follower.cpp | 26 +------------------ 1 file changed, 1 insertion(+), 25 deletions(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 6a03808859..0d01d456c3 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -471,34 +471,10 @@ WaypointFollower::convertGPSPosesToMapPoses( } continue; } else { - // from_ll_to_map_client_ converted the point from LL to Map frames - // but it actually did not consider the possible yaw offset between utm and map frames ; - // "https://github.com/cra-ros-pkg/robot_localization/blob/ - // 79162b2ac53a112c51d23859c499e8438cf9938e/src/navsat_transform.cpp#L394" - // see above link on how they set rotation between UTM and - // Map to Identity , where actually it might not be - geometry_msgs::msg::TransformStamped utm_to_map_transform; - try { - utm_to_map_transform = tf_buffer_->lookupTransform(utm_frame_id_, global_frame_id_, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), - "Exception in getting %s -> %s transform: %s", - utm_frame_id_.c_str(), global_frame_id_.c_str(), ex.what()); - } - // we need to consider the possible yaw_offset between utm and map here, - // rotate x , y with amount of yaw - tf2::Quaternion utm_to_map_quat; - tf2::fromMsg(utm_to_map_transform.transform.rotation, utm_to_map_quat); - const tf2::Matrix3x3 m(utm_to_map_quat); - const tf2::Vector3 point({response->map_point.x, response->map_point.y, response->map_point.z}); - auto corrected_point = m*point; geometry_msgs::msg::PoseStamped curr_pose_map_frame; curr_pose_map_frame.header.frame_id = global_frame_id_; curr_pose_map_frame.header.stamp = this->now(); - curr_pose_map_frame.pose.position.x = corrected_point[0]; - curr_pose_map_frame.pose.position.y = corrected_point[1]; - curr_pose_map_frame.pose.position.z = response->map_point.z; + curr_pose_map_frame.pose.position = response->map_point; curr_pose_map_frame.pose.orientation = curr_geopose.orientation; poses_in_map_frame_vector.push_back(curr_pose_map_frame); } From 84999e1fc9d04c47a949462caea1d903ac78f39a Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 15 Jun 2022 19:39:05 +0000 Subject: [PATCH 42/70] Fix API change --- nav2_waypoint_follower/src/waypoint_follower.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index df846dfa78..630d6c6b9a 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -339,7 +339,7 @@ void WaypointFollower::followWaypointsHandler( if (goal_index >= poses.size()) { RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", - goal->poses.size()); + poses.size()); result->missed_waypoints = failed_ids_; action_server->succeeded_current(result); failed_ids_.clear(); From 879b4e6cf81e7c08cf9a62c879bc517c1325cc55 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 17 Jun 2022 16:58:17 -0500 Subject: [PATCH 43/70] Fix styling --- .../waypoint_follower.hpp | 7 +++---- .../src/waypoint_follower.cpp | 19 +++++++++---------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 69f66cac2c..a997919c7b 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -58,7 +58,7 @@ enum class ActionStatus */ class WaypointFollower : public nav2_util::LifecycleNode { -public: + public: using ActionT = nav2_msgs::action::FollowWaypoints; using ClientT = nav2_msgs::action::NavigateToPose; using ActionServer = nav2_util::SimpleActionServer; @@ -175,7 +175,6 @@ class WaypointFollower : public nav2_util::LifecycleNode int loop_rate_; bool stop_on_failure_; std::string global_frame_id_{"map"}; - std::string utm_frame_id_{"utm"}; /** * @brief Callback executed when a parameter change is detected @@ -197,8 +196,8 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; - std::unique_ptr>> from_ll_to_map_client_; - double transform_tolerance_; + std::unique_ptr>> from_ll_to_map_client_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 630d6c6b9a..319179f3d3 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -36,9 +36,7 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); - declare_parameter("transform_tolerance", 0.1); declare_parameter("global_frame_id", global_frame_id_); - declare_parameter("utm_frame_id", utm_frame_id_); nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), @@ -62,12 +60,9 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); - transform_tolerance_ = get_parameter("transform_tolerance").as_double(); global_frame_id_ = get_parameter("global_frame_id").as_string(); - utm_frame_id_ = get_parameter("utm_frame_id").as_string(); global_frame_id_ = nav2_util::strip_leading_slash(global_frame_id_); - utm_frame_id_ = nav2_util::strip_leading_slash(utm_frame_id_); callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, @@ -89,7 +84,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) "follow_waypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); from_ll_to_map_client_ = std::make_unique< - nav2_util::ServiceClient>>( + nav2_util::ServiceClient>>( "/fromLL", node); @@ -98,7 +94,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "follow_gps_waypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); + "follow_gps_waypoints", + std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); // used for transfroming orientation of GPS poses to map frame tf_buffer_ = std::make_shared(node->get_clock()); @@ -442,7 +439,7 @@ std::vector WaypointFollower::convertGPSPosesToMapPoses( const std::vector & gps_poses) { - RCLCPP_INFO(this->get_logger(), "Converting GPS waypoints to %s Frame..", + RCLCPP_INFO(this->get_logger(), "Converting GPS waypoints to %s Frame..", global_frame_id_.c_str()); std::vector poses_in_map_frame_vector; @@ -460,13 +457,15 @@ WaypointFollower::convertGPSPosesToMapPoses( this->get_logger(), "fromLL service of robot_localization could not convert %i th GPS waypoint to" "%s frame, going to skip this point!" - "Make sure you have run navsat_transform_node of robot_localization", waypoint_index, global_frame_id_.c_str()); + "Make sure you have run navsat_transform_node of robot_localization", + waypoint_index, global_frame_id_.c_str()); if (stop_on_failure_) { RCLCPP_ERROR( this->get_logger(), "Conversion of %i th GPS waypoint to" "%s frame failed and stop_on_failure is set to true" - "Not going to execute any of waypoints, exiting with failure!", waypoint_index, global_frame_id_.c_str()); + "Not going to execute any of waypoints, exiting with failure!", + waypoint_index, global_frame_id_.c_str()); return std::vector(); } continue; From 8cc6311a285f538e23f995cf704ae9f81aa29866 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 17 Jun 2022 17:02:27 -0500 Subject: [PATCH 44/70] Minor docs fixes --- nav2_waypoint_follower/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 0284b15824..5d6a049857 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -34,7 +34,7 @@ Neither is better than the other, it highly depends on the tasks your robot(s) a `robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude) to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. -The action msg definition for GPS waypoint following can be found [here](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/action/FollowGPSWaypoints.action). +The action msg definition for GPS waypoint following can be found [here](../nav2_msgs/action/FollowGPSWaypoints.action). In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. For instance, @@ -45,4 +45,4 @@ rclcpp_action::Client::SharedPtr gps_waypoint_follower_action_client_; gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "follow_gps_waypoints"); ``` -All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file +All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file From 03cca6078356d2e00bd8f38e0245ddb4fab7e1aa Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 17 Jun 2022 17:46:47 -0500 Subject: [PATCH 45/70] Fix style divergences --- .../waypoint_follower.hpp | 27 ++++++++++++------- .../src/waypoint_follower.cpp | 6 ++--- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index a997919c7b..995f51270d 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -156,16 +156,25 @@ class WaypointFollower : public nav2_util::LifecycleNode template void goalResponseCallback(const T & goal); - /** - * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. - * Constructs a vector of stamped poses in map frame and returns them. - * - * @param gps_poses - * @return std::vector - */ - std::vector convertGPSPosesToMapPoses( - const std::vector& gps_poses); + /** + * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. + * Constructs a vector of stamped poses in map frame and returns them. + * + * @param gps_poses, from the action server + * @return std::vector + */ + std::vector convertGPSPosesToMapPoses( + const std::vector& gps_poses); + + /** + * @brief get the latest poses on the action server goal. If they are GPS poses, + * convert them to the global cartesian frame using /fromLL robot localization + * server + * + * @param action_server, to which the goal was sent + * @return std::vector + */ template std::vector getLatestGoalPoses(const T & action_server); diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 319179f3d3..a866f2f92b 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -147,7 +147,6 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) action_server_->deactivate(); gps_action_server_->deactivate(); dyn_params_handler_.reset(); - // destroy bond connection destroyBond(); @@ -181,8 +180,7 @@ std::vector WaypointFollower::getLatestGoalPose std::vector poses; // compile time static check to decide which block of code to be built - if constexpr (std::is_same>::value) - { + if constexpr (std::is_same>::value) { // If normal waypoint following callback was called, we build here poses = action_server->get_current_goal()->poses; } else { @@ -440,7 +438,7 @@ WaypointFollower::convertGPSPosesToMapPoses( const std::vector & gps_poses) { RCLCPP_INFO(this->get_logger(), "Converting GPS waypoints to %s Frame..", - global_frame_id_.c_str()); + global_frame_id_.c_str()); std::vector poses_in_map_frame_vector; int waypoint_index = 0; From 8e87793c44cce7603cab1e5d27ff3a7a6fcaa57e Mon Sep 17 00:00:00 2001 From: pepisg Date: Tue, 21 Jun 2022 14:46:58 -0500 Subject: [PATCH 46/70] Style fixes --- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 8 ++++---- nav2_waypoint_follower/src/waypoint_follower.cpp | 3 ++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 995f51270d..f81763e73a 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -125,8 +125,8 @@ class WaypointFollower : public nav2_util::LifecycleNode * @param feedback * @param result */ - template - void followWaypointsHandler(const T& action_server, const V& feedback, const Z& result); + template + void followWaypointsHandler(const T& action_server, const V& feedback, const Z& result); /** * @brief Action server callbacks @@ -164,7 +164,7 @@ class WaypointFollower : public nav2_util::LifecycleNode * @return std::vector */ std::vector convertGPSPosesToMapPoses( - const std::vector& gps_poses); + const std::vector& gps_poses); /** @@ -206,7 +206,7 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server for GPS waypoint following std::unique_ptr gps_action_server_; std::unique_ptr>> from_ll_to_map_client_; + std::shared_ptr>> from_ll_to_map_client_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a866f2f92b..5d9a9cba78 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -437,7 +437,8 @@ std::vector WaypointFollower::convertGPSPosesToMapPoses( const std::vector & gps_poses) { - RCLCPP_INFO(this->get_logger(), "Converting GPS waypoints to %s Frame..", + RCLCPP_INFO( + this->get_logger(), "Converting GPS waypoints to %s Frame..", global_frame_id_.c_str()); std::vector poses_in_map_frame_vector; From 231f70563b83598b02cec86e655f29ab9edc8528 Mon Sep 17 00:00:00 2001 From: pepisg Date: Tue, 21 Jun 2022 17:35:02 -0500 Subject: [PATCH 47/70] Style fixes v2 --- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index f81763e73a..f7be27020b 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -125,8 +125,8 @@ class WaypointFollower : public nav2_util::LifecycleNode * @param feedback * @param result */ - template - void followWaypointsHandler(const T& action_server, const V& feedback, const Z& result); + template + void followWaypointsHandler(const T & action_server, const V & feedback, const Z & result); /** * @brief Action server callbacks @@ -164,7 +164,7 @@ class WaypointFollower : public nav2_util::LifecycleNode * @return std::vector */ std::vector convertGPSPosesToMapPoses( - const std::vector& gps_poses); + const std::vector & gps_poses); /** From 6a3a5fb09b99ed7ef25f4f8c85fc664ddae9d49f Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 22 Jun 2022 10:02:05 -0500 Subject: [PATCH 48/70] Style fixes v3 --- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index f7be27020b..33c145c88c 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -58,7 +58,7 @@ enum class ActionStatus */ class WaypointFollower : public nav2_util::LifecycleNode { - public: +public: using ActionT = nav2_msgs::action::FollowWaypoints; using ClientT = nav2_msgs::action::NavigateToPose; using ActionServer = nav2_util::SimpleActionServer; From cf4ed6e82c2d270b897fad33baa1c5f8b337b33d Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 23 Jun 2022 21:55:22 -0500 Subject: [PATCH 49/70] Remove unused variables and timestam overrides --- nav2_waypoint_follower/CMakeLists.txt | 2 -- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 5 ----- nav2_waypoint_follower/src/waypoint_follower.cpp | 6 ------ 3 files changed, 13 deletions(-) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index d09de8fc3b..9f262726fd 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) -# Default to C++17 -set(CMAKE_CXX_STANDARD 17) # Try for OpenCV 4.X, but settle for whatever is installed find_package(OpenCV 4 QUIET) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 33c145c88c..c79d24d732 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -179,7 +179,6 @@ class WaypointFollower : public nav2_util::LifecycleNode std::vector getLatestGoalPoses(const T & action_server); // Common vars used for both GPS and cartesian point following - rclcpp::Node::SharedPtr client_node_; std::vector failed_ids_; int loop_rate_; bool stop_on_failure_; @@ -215,10 +214,6 @@ class WaypointFollower : public nav2_util::LifecycleNode waypoint_task_executor_; std::string waypoint_task_executor_id_; std::string waypoint_task_executor_type_; - - // tf buffer to get transfroms - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 5d9a9cba78..f9ea4bff91 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -97,10 +97,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) "follow_gps_waypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); - // used for transfroming orientation of GPS poses to map frame - tf_buffer_ = std::make_shared(node->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( this, @@ -258,7 +254,6 @@ void WaypointFollower::followWaypointsHandler( new_goal = false; ClientT::Goal client_goal; client_goal.pose = poses[goal_index]; - client_goal.pose.header.stamp = this->now(); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback = @@ -329,7 +324,6 @@ void WaypointFollower::followWaypointsHandler( { // Update server state goal_index++; - poses[goal_index].header.stamp = this->now(); new_goal = true; if (goal_index >= poses.size()) { RCLCPP_INFO( From 46fce6ea6039d0df7e864eb5623e834aca3849a8 Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 26 Sep 2022 18:41:17 +0000 Subject: [PATCH 50/70] restore goal timestamp override --- nav2_waypoint_follower/src/waypoint_follower.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 7fb7f2db34..4c5d89e1b9 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -254,6 +254,7 @@ void WaypointFollower::followWaypointsHandler( new_goal = false; ClientT::Goal client_goal; client_goal.pose = poses[goal_index]; + client_goal.pose.header.stamp = this->now(); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback = From e84158a8af76ac9f99571561a036e53dbd44d2cd Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 28 Sep 2022 22:27:45 +0000 Subject: [PATCH 51/70] WIP: Add follow gps waypoints test --- nav2_system_tests/CMakeLists.txt | 1 + .../src/gps_navigation/CMakeLists.txt | 11 + .../gps_navigation/dual_ekf_navsat.launch.py | 65 +++ .../dual_ekf_navsat_params.yaml | 127 ++++ .../gps_navigation/nav2_no_map_params.yaml | 291 ++++++++++ .../src/gps_navigation/test_case_py.launch.py | 123 ++++ .../src/gps_navigation/tester.py | 195 +++++++ .../worlds/turtlebot3_ros2_demo_gps.world | 542 ++++++++++++++++++ 8 files changed, 1355 insertions(+) create mode 100644 nav2_system_tests/src/gps_navigation/CMakeLists.txt create mode 100644 nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py create mode 100644 nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml create mode 100644 nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml create mode 100755 nav2_system_tests/src/gps_navigation/test_case_py.launch.py create mode 100755 nav2_system_tests/src/gps_navigation/tester.py create mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6d56df1ef7..473d2995dd 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -61,6 +61,7 @@ if(BUILD_TESTING) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) + add_subdirectory(src/gps_navigation) add_subdirectory(src/behaviors/spin) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/backup) diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt new file mode 100644 index 0000000000..7b6c3d667b --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/CMakeLists.txt @@ -0,0 +1,11 @@ +ament_add_test(test_waypoint_follower + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_py.launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_gps.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py new file mode 100644 index 0000000000..c640a477a6 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py @@ -0,0 +1,65 @@ +# Copyright 2018 Open Source Robotics Foundation, Inc. +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +import launch_ros.actions +import os +import yaml +from launch.substitutions import EnvironmentVariable +import pathlib +import launch.actions +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'dual_ekf_navsat_params.yaml') + os.environ['FILE_PATH'] = str(launch_dir) + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + 'output_final_position', + default_value='false'), + launch.actions.DeclareLaunchArgument( + 'output_location', + default_value='~/dual_ekf_navsat_example_debug.txt'), + + launch_ros.actions.Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_odom', + output='screen', + parameters=[params_file, {"use_sim_time": True}], + remappings=[('odometry/filtered', 'odometry/local')] + ), + launch_ros.actions.Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_map', + output='screen', + parameters=[params_file, {"use_sim_time": True}], + remappings=[('odometry/filtered', 'odometry/global')] + ), + launch_ros.actions.Node( + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform', + output='screen', + parameters=[params_file, {"use_sim_time": True}], + remappings=[('imu/data', 'imu/data'), + ('gps/fix', 'gps/fix'), + ('gps/filtered', 'gps/filtered'), + ('odometry/gps', 'odometry/gps'), + ('odometry/filtered', 'odometry/global')] + + ) +]) \ No newline at end of file diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml new file mode 100644 index 0000000000..6783a7b1db --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml @@ -0,0 +1,127 @@ +# For parameter descriptions, please refer to the template parameter files for each node. + +ekf_filter_node_odom: + ros__parameters: + frequency: 30.0 + two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + odom0: odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: false + odom0_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3] + +ekf_filter_node_map: + ros__parameters: + frequency: 30.0 + two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + odom0: odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: false + odom0_relative: false + + odom1: odometry/gps + odom1_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom1_queue_size: 10 + odom1_differential: false + odom1_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3] + +navsat_transform: + ros__parameters: + frequency: 30.0 + delay: 3.0 + magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998 + yaw_offset: 0.0 + zero_altitude: true + broadcast_utm_transform: true + publish_filtered_gps: true + use_odometry_yaw: true + wait_for_datum: false diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml new file mode 100644 index 0000000000..13f8b05420 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -0,0 +1,291 @@ +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + # When using GPS navigation you will potentially traverse huge environments which are not practical to + # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to + # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 + rolling_window: True + width: 80 + height: 80 + track_unknown_space: true + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + log_level: debug + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py new file mode 100755 index 0000000000..cfe34971e0 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -0,0 +1,123 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + world = os.getenv('TEST_WORLD') + + # bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + # 'behavior_trees', + # os.getenv('BT_NAVIGATOR_XML')) + + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'nav2_no_map_params.yaml') + + bringup_dir = get_package_share_directory('nav2_bringup') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + # context = LaunchContext() + # new_yaml = configured_params.perform(context) + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['-0.32', '0', '0.068', '0', '0', '0', 'base_link', 'imu_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'gps_link']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'navigation_launch.py')), + launch_arguments={ + 'use_sim_time': 'True', + 'params_file': configured_params, + 'autostart': 'True'}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'dual_ekf_navsat.launch.py')), + launch_arguments={ + 'use_sim_time': 'True', + 'params_file': configured_params, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')], + name='tester_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py new file mode 100755 index 0000000000..4d6a31bf3f --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -0,0 +1,195 @@ +#! /usr/bin/env python3 +# Copyright 2019 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import time + +from action_msgs.msg import GoalStatus +from geographic_msgs.msg import GeoPose +from nav2_msgs.action import FollowGPSWaypoints +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class WaypointFollowerTest(Node): + + def __init__(self): + super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') + self.waypoints = None + self.action_client = ActionClient(self, FollowGPSWaypoints, 'follow_gps_waypoints') + self.goal_handle = None + + def setWaypoints(self, waypoints): + self.waypoints = [] + for wp in waypoints: + msg = GeoPose() + msg.position.latitude = wp[0] + msg.position.longitude = wp[1] + msg.orientation.w = 1.0 + self.waypoints.append(msg) + + def run(self, block): + if not self.waypoints: + rclpy.error_msg('Did not set valid waypoints before running test!') + return False + + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'follow_waypoints' action server not available, waiting...") + + action_request = FollowGPSWaypoints.Goal() + action_request.gps_poses = self.waypoints + + self.info_msg('Sending goal request...') + send_goal_future = self.action_client.send_goal_async(action_request) + try: + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + if not block: + return True + + get_result_future = self.goal_handle.get_result_async() + + self.info_msg("Waiting for 'follow_waypoints' action to complete") + try: + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + result = get_result_future.result().result + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + if len(result.missed_waypoints) > 0: + self.info_msg('Goal failed to process all waypoints,' + ' missed {0} wps.'.format(len(result.missed_waypoints))) + return False + + self.info_msg('Goal succeeded!') + return True + + + def shutdown(self): + self.info_msg('Shutting down') + + self.action_client.destroy() + self.info_msg('Destroyed follow_waypoints action client') + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + rclpy.spin_until_future_complete(self, future) + future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'{transition_service} service call failed {e!r}') + + self.info_msg(f'{transition_service} finished') + + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + rclpy.spin_until_future_complete(self, future) + future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'{transition_service} service call failed {e!r}') + + self.info_msg(f'{transition_service} finished') + + def cancel_goal(self): + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + + def info_msg(self, msg: str): + self.get_logger().info(msg) + + def warn_msg(self, msg: str): + self.get_logger().warn(msg) + + def error_msg(self, msg: str): + self.get_logger().error(msg) + + +def main(argv=sys.argv[1:]): + rclpy.init() + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + wps = [[55.944831, -3.186998], [55.944818, -3.187075], [55.944782, -3.187060]] + + test = WaypointFollowerTest() + test.setWaypoints(wps) + + # wait for poseCallback + + result = test.run(True) + assert result + + # preempt with new point + test.setWaypoints([wps[0]]) + result = test.run(False) + time.sleep(2) + test.setWaypoints([wps[1]]) + result = test.run(False) + + # cancel + time.sleep(2) + test.cancel_goal() + + # a failure case + time.sleep(2) + test.setWaypoints([[35.0, -118.0]]) + result = test.run(True) + assert not result + result = not result + + test.shutdown() + test.info_msg('Done Shutting Down.') + + if not result: + test.info_msg('Exiting failed') + exit(1) + else: + test.info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world new file mode 100644 index 0000000000..59170a7b47 --- /dev/null +++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world @@ -0,0 +1,542 @@ + + + + + + + EARTH_WGS84 + ENU + 55.944831 + -3.186998 + 0.0 + 180.0 + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + -2.0 -0.5 0.01 0.0 0.0 0.0 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=/imu/data + + + + + + + 0 0 0 0 0 0 + + true + 1 + 0 0 0 0 0 0 + + + + + 0.0 + 0.1 + + + + + 0.0 + 0.0 + + + + + + + ~/out:=/gps/fix + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.064 0 0.121 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.032 0 0.171 0 0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + false + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + imu_link + -0.032 0 0.068 0 0 0 + + 0 0 1 + + + + + base_link + gps_link + 0 0 0 0 0 0 + + 0 0 1 + + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + + + + /tf:=tf + + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + + true + false + false + + odom + base_link + + + + + + + ~/out:=joint_states + + 250 + wheel_left_joint + wheel_right_joint + + + + + + + From 939756ee29a15fdccecff53edd64ee0258e679a4 Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 29 Sep 2022 16:58:58 +0000 Subject: [PATCH 52/70] Style fixes and gazebo world inertia fix --- .../gps_navigation/dual_ekf_navsat.launch.py | 88 ++- .../src/gps_navigation/test_case_py.launch.py | 162 ++--- .../src/gps_navigation/tester.py | 70 +-- .../worlds/turtlebot3_ros2_demo_gps.world | 575 +++++++++--------- 4 files changed, 452 insertions(+), 443 deletions(-) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py index c640a477a6..852436615b 100644 --- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py @@ -14,52 +14,50 @@ from launch import LaunchDescription import launch_ros.actions import os -import yaml -from launch.substitutions import EnvironmentVariable -import pathlib import launch.actions -from launch.actions import DeclareLaunchArgument -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): launch_dir = os.path.dirname(os.path.realpath(__file__)) - params_file = os.path.join(launch_dir, 'dual_ekf_navsat_params.yaml') - os.environ['FILE_PATH'] = str(launch_dir) - return LaunchDescription([ - launch.actions.DeclareLaunchArgument( - 'output_final_position', - default_value='false'), - launch.actions.DeclareLaunchArgument( - 'output_location', - default_value='~/dual_ekf_navsat_example_debug.txt'), - - launch_ros.actions.Node( - package='robot_localization', - executable='ekf_node', - name='ekf_filter_node_odom', - output='screen', - parameters=[params_file, {"use_sim_time": True}], - remappings=[('odometry/filtered', 'odometry/local')] - ), - launch_ros.actions.Node( - package='robot_localization', - executable='ekf_node', - name='ekf_filter_node_map', - output='screen', - parameters=[params_file, {"use_sim_time": True}], - remappings=[('odometry/filtered', 'odometry/global')] - ), - launch_ros.actions.Node( - package='robot_localization', - executable='navsat_transform_node', - name='navsat_transform', - output='screen', - parameters=[params_file, {"use_sim_time": True}], - remappings=[('imu/data', 'imu/data'), - ('gps/fix', 'gps/fix'), - ('gps/filtered', 'gps/filtered'), - ('odometry/gps', 'odometry/gps'), - ('odometry/filtered', 'odometry/global')] - - ) -]) \ No newline at end of file + params_file = os.path.join(launch_dir, "dual_ekf_navsat_params.yaml") + os.environ["FILE_PATH"] = str(launch_dir) + return LaunchDescription( + [ + launch.actions.DeclareLaunchArgument( + "output_final_position", default_value="false" + ), + launch.actions.DeclareLaunchArgument( + "output_location", default_value="~/dual_ekf_navsat_example_debug.txt" + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_odom", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/local")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_map", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/global")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="navsat_transform_node", + name="navsat_transform", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[ + ("imu/data", "imu/data"), + ("gps/fix", "gps/fix"), + ("gps/filtered", "gps/filtered"), + ("odometry/gps", "odometry/gps"), + ("odometry/filtered", "odometry/global"), + ], + ), + ] + ) diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index cfe34971e0..3f18d5b7ae 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -20,8 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable -from launch.launch_context import LaunchContext +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -30,87 +33,98 @@ def generate_launch_description(): - world = os.getenv('TEST_WORLD') - - # bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - # 'behavior_trees', - # os.getenv('BT_NAVIGATOR_XML')) + world = os.getenv("TEST_WORLD") launch_dir = os.path.dirname(os.path.realpath(__file__)) - params_file = os.path.join(launch_dir, 'nav2_no_map_params.yaml') - - bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(launch_dir, "nav2_no_map_params.yaml") + bringup_dir = get_package_share_directory("nav2_bringup") - # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - # context = LaunchContext() - # new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['-0.32', '0', '0.068', '0', '0', '0', 'base_link', 'imu_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'gps_link']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'navigation_launch.py')), - launch_arguments={ - 'use_sim_time': 'True', - 'params_file': configured_params, - 'autostart': 'True'}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'dual_ekf_navsat.launch.py')), - launch_arguments={ - 'use_sim_time': 'True', - 'params_file': configured_params, - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key="", param_rewrites="", convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + SetEnvironmentVariable("RCUTILS_LOGGING_USE_STDOUT", "1"), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + "gzserver", + "-s", + "libgazebo_ros_init.so", + "--minimal_comms", + world, + ], + output="screen", + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_footprint", "base_link"], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_scan"], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=[ + "-0.32", + "0", + "0.068", + "0", + "0", + "0", + "base_link", + "imu_link", + ], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_link", "gps_link"], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, "launch", "navigation_launch.py") + ), + launch_arguments={ + "use_sim_time": "True", + "params_file": configured_params, + "autostart": "True", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "dual_ekf_navsat.launch.py") + ), + launch_arguments={ + "use_sim_time": "True", + "params_file": configured_params, + "autostart": "True", + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')], - name='tester_node', - output='screen') + cmd=[os.path.join(os.getenv("TEST_DIR"), "tester.py")], + name="tester_node", + output="screen", + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) @@ -119,5 +133,5 @@ def main(argv=sys.argv[1:]): return lts.run(ls) -if __name__ == '__main__': +if __name__ == "__main__": sys.exit(main()) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 4d6a31bf3f..77e54fc8da 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -24,16 +24,15 @@ import rclpy from rclpy.action import ActionClient from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile class WaypointFollowerTest(Node): - def __init__(self): - super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') + super().__init__(node_name="nav2_gps_waypoint_tester", namespace="") self.waypoints = None - self.action_client = ActionClient(self, FollowGPSWaypoints, 'follow_gps_waypoints') + self.action_client = ActionClient( + self, FollowGPSWaypoints, "follow_gps_waypoints" + ) self.goal_handle = None def setWaypoints(self, waypoints): @@ -47,7 +46,7 @@ def setWaypoints(self, waypoints): def run(self, block): if not self.waypoints: - rclpy.error_msg('Did not set valid waypoints before running test!') + rclpy.error_msg("Did not set valid waypoints before running test!") return False while not self.action_client.wait_for_server(timeout_sec=1.0): @@ -56,19 +55,19 @@ def run(self, block): action_request = FollowGPSWaypoints.Goal() action_request.gps_poses = self.waypoints - self.info_msg('Sending goal request...') + self.info_msg("Sending goal request...") send_goal_future = self.action_client.send_goal_async(action_request) try: rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() except Exception as e: # noqa: B902 - self.error_msg(f'Service call failed {e!r}') + self.error_msg(f"Service call failed {e!r}") if not self.goal_handle.accepted: - self.error_msg('Goal rejected') + self.error_msg("Goal rejected") return False - self.info_msg('Goal accepted') + self.info_msg("Goal accepted") if not block: return True @@ -80,46 +79,31 @@ def run(self, block): status = get_result_future.result().status result = get_result_future.result().result except Exception as e: # noqa: B902 - self.error_msg(f'Service call failed {e!r}') + self.error_msg(f"Service call failed {e!r}") if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg(f'Goal failed with status code: {status}') + self.info_msg(f"Goal failed with status code: {status}") return False if len(result.missed_waypoints) > 0: - self.info_msg('Goal failed to process all waypoints,' - ' missed {0} wps.'.format(len(result.missed_waypoints))) + self.info_msg( + "Goal failed to process all waypoints," + " missed {0} wps.".format(len(result.missed_waypoints)) + ) return False - self.info_msg('Goal succeeded!') + self.info_msg("Goal succeeded!") return True - def shutdown(self): - self.info_msg('Shutting down') + self.info_msg("Shutting down") self.action_client.destroy() - self.info_msg('Destroyed follow_waypoints action client') - - transition_service = 'lifecycle_manager_navigation/manage_nodes' - mgr_client = self.create_client(ManageLifecycleNodes, transition_service) - while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(f'{transition_service} service not available, waiting...') - - req = ManageLifecycleNodes.Request() - req.command = ManageLifecycleNodes.Request().SHUTDOWN - future = mgr_client.call_async(req) - try: - rclpy.spin_until_future_complete(self, future) - future.result() - except Exception as e: # noqa: B902 - self.error_msg(f'{transition_service} service call failed {e!r}') - - self.info_msg(f'{transition_service} finished') + self.info_msg("Destroyed follow_waypoints action client") - transition_service = 'lifecycle_manager_localization/manage_nodes' + transition_service = "lifecycle_manager_navigation/manage_nodes" mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(f'{transition_service} service not available, waiting...') + self.info_msg(f"{transition_service} service not available, waiting...") req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -128,9 +112,9 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() except Exception as e: # noqa: B902 - self.error_msg(f'{transition_service} service call failed {e!r}') + self.error_msg(f"{transition_service} service call failed {e!r}") - self.info_msg(f'{transition_service} finished') + self.info_msg(f"{transition_service} finished") def cancel_goal(self): cancel_future = self.goal_handle.cancel_goal_async() @@ -157,7 +141,7 @@ def main(argv=sys.argv[1:]): test = WaypointFollowerTest() test.setWaypoints(wps) - # wait for poseCallback + # wait for poseCallback result = test.run(True) assert result @@ -181,15 +165,15 @@ def main(argv=sys.argv[1:]): result = not result test.shutdown() - test.info_msg('Done Shutting Down.') + test.info_msg("Done Shutting Down.") if not result: - test.info_msg('Exiting failed') + test.info_msg("Exiting failed") exit(1) else: - test.info_msg('Exiting passed') + test.info_msg("Exiting passed") exit(0) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world index 59170a7b47..94b72499a2 100644 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world +++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world @@ -153,7 +153,20 @@ - 0 0 0 0 0 0 + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + true 1 @@ -249,293 +262,293 @@ sensor_msgs/LaserScan - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - + - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/left_tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - + - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/right_tire.dae - 0.001 0.001 0.001 - - - - + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - + - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - imu_link - -0.032 0 0.068 0 0 0 - - 0 0 1 - - - - - base_link - gps_link - 0 0 0 0 0 0 - - 0 0 1 - - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - - true - false - false - - odom - base_link - - - - - - - ~/out:=joint_states - - 250 - wheel_left_joint - wheel_right_joint - - - + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + imu_link + -0.032 0 0.068 0 0 0 + + 0 0 1 + + + + + base_link + gps_link + -0.05 0 0.05 0 0 0 + + 0 0 1 + + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + + + + /tf:=tf + + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + + true + false + false + + odom + base_link + + + + + + + ~/out:=joint_states + + 250 + wheel_left_joint + wheel_right_joint + + + From 5f95c13dca72b16ab202c09be60184551fca62ec Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 29 Sep 2022 18:58:38 +0000 Subject: [PATCH 53/70] Reduce velocity smoother timeout --- nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml | 2 +- nav2_system_tests/src/gps_navigation/test_case_py.launch.py | 5 ----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 13f8b05420..cc34338642 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -288,4 +288,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 + velocity_timeout: 0.5 diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index 3f18d5b7ae..cd90fdb620 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -107,11 +107,6 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(launch_dir, "dual_ekf_navsat.launch.py") ), - launch_arguments={ - "use_sim_time": "True", - "params_file": configured_params, - "autostart": "True", - }.items(), ), ] ) From 839655f15dd434fcd60e531538e89423061c60e6 Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 29 Sep 2022 22:17:28 +0000 Subject: [PATCH 54/70] empty commit to rerun tests From 5226c0b1db038b85a7bb440d7f5be3c8ad362fd6 Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 3 Oct 2022 18:21:12 +0000 Subject: [PATCH 55/70] Increment circle ci cache idx --- .circleci/config.yml | 6 +++--- nav2_system_tests/src/gps_navigation/CMakeLists.txt | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index ede7b367e9..dd94d2e7c5 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v12\ + - "<< parameters.key >>-v13\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v12\ + - "<< parameters.key >>-v13\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v12\ + key: "<< parameters.key >>-v13\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt index 7b6c3d667b..3342b6cd58 100644 --- a/nav2_system_tests/src/gps_navigation/CMakeLists.txt +++ b/nav2_system_tests/src/gps_navigation/CMakeLists.txt @@ -1,4 +1,4 @@ -ament_add_test(test_waypoint_follower +ament_add_test(test_gps_waypoint_follower GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_py.launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" From 96215ef338a51fd04ebbb18e646024603639aa79 Mon Sep 17 00:00:00 2001 From: pepisg Date: Tue, 4 Oct 2022 20:54:06 +0000 Subject: [PATCH 56/70] Remove extra space in cmakelists.txt --- nav2_waypoint_follower/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 9f262726fd..1d5405878b 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,7 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) - # Try for OpenCV 4.X, but settle for whatever is installed find_package(OpenCV 4 QUIET) if(NOT OpenCV_FOUND) From 376f5998b3293dadcaefbd77bfe5400403aff60b Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 30 Dec 2022 14:13:10 +0000 Subject: [PATCH 57/70] Fix wrong usage of the global action server --- nav2_waypoint_follower/src/waypoint_follower.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index afbd789b69..382262eb81 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -228,7 +228,7 @@ void WaypointFollower::followWaypointsHandler( callback_group_executor_.spin_until_future_complete(cancel_future); // for result callback processing callback_group_executor_.spin_some(); - action_server_->terminate_all(); + action_server->terminate_all(); return; } From 5ec4d8c13abaca367734562c32349652bbf8eda7 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 30 Dec 2022 16:27:25 +0000 Subject: [PATCH 58/70] update follow gps waypoints action definition --- nav2_msgs/action/FollowGPSWaypoints.action | 8 +++++++- nav2_waypoint_follower/src/waypoint_follower.cpp | 6 +++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action index d4e39ca9a0..c933326777 100644 --- a/nav2_msgs/action/FollowGPSWaypoints.action +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -1,8 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 + #goal definition geographic_msgs/GeoPose[] gps_poses --- #result definition -int32[] missed_waypoints +MissedWaypoint[] missed_waypoints --- #feedback uint32 current_waypoint diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 1f9ec9a639..8bc9cfc89e 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -279,7 +279,7 @@ void WaypointFollower::followWaypointsHandler( if (current_goal_status_.status == ActionStatus::FAILED) { nav2_msgs::msg::MissedWaypoint missedWaypoint; missedWaypoint.index = goal_index; - missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.goal = poses[goal_index]; missedWaypoint.error_code = current_goal_status_.error_code; result->missed_waypoints.push_back(missedWaypoint); @@ -309,7 +309,7 @@ void WaypointFollower::followWaypointsHandler( if (!is_task_executed) { nav2_msgs::msg::MissedWaypoint missedWaypoint; missedWaypoint.index = goal_index; - missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.goal = poses[goal_index]; missedWaypoint.error_code = nav2_msgs::action::FollowWaypoints::Goal::TASK_EXECUTOR_FAILED; result->missed_waypoints.push_back(missedWaypoint); } @@ -339,7 +339,7 @@ void WaypointFollower::followWaypointsHandler( if (goal_index >= poses.size()) { RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", - goal->poses.size()); + poses.size()); action_server->succeeded_current(result); current_goal_status_.error_code = 0; return; From cefd0bf94918af4a41f5055f7bb5d67147c646ef Mon Sep 17 00:00:00 2001 From: root Date: Sun, 13 Aug 2023 02:15:04 +0000 Subject: [PATCH 59/70] Fix action definition and looping --- nav2_msgs/action/FollowGPSWaypoints.action | 11 +++++++---- nav2_waypoint_follower/src/waypoint_follower.cpp | 6 +++++- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action index c933326777..707e9a2b37 100644 --- a/nav2_msgs/action/FollowGPSWaypoints.action +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -1,13 +1,16 @@ +#goal definition +uint32 number_of_loops +uint32 goal_index 0 +geographic_msgs/GeoPose[] gps_poses +--- +#result definition + # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 uint16 UNKNOWN=600 uint16 TASK_EXECUTOR_FAILED=601 -#goal definition -geographic_msgs/GeoPose[] gps_poses ---- -#result definition MissedWaypoint[] missed_waypoints --- #feedback diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 877ace54af..10f241c900 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -195,6 +195,10 @@ void WaypointFollower::followWaypointsHandler( { auto goal = action_server->get_current_goal(); + // handling loops + unsigned int current_loop_no = 0; + auto no_of_loops = goal->number_of_loops; + std::vector poses; poses = getLatestGoalPoses(action_server); @@ -344,7 +348,7 @@ void WaypointFollower::followWaypointsHandler( RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", poses.size()); - action_server_->succeeded_current(result); + action_server->succeeded_current(result); current_goal_status_.error_code = 0; return; } From ed961015cc08c7cfe6167e6d36ebc969cf26e4b4 Mon Sep 17 00:00:00 2001 From: root Date: Sun, 13 Aug 2023 23:20:28 +0000 Subject: [PATCH 60/70] update params for the unit testing --- .../gps_navigation/nav2_no_map_params.yaml | 142 ++++++++++-------- 1 file changed, 79 insertions(+), 63 deletions(-) diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index cc34338642..2539cef7b5 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -5,57 +5,70 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: @@ -64,7 +77,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] @@ -177,14 +190,15 @@ global_costmap: global_frame: map robot_base_frame: base_link robot_radius: 0.22 - resolution: 0.05 + resolution: 0.1 # When using GPS navigation you will potentially traverse huge environments which are not practical to # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 rolling_window: True - width: 80 - height: 80 + width: 50 + height: 50 track_unknown_space: true + # no static map plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -200,20 +214,20 @@ global_costmap: raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True + # outdoors there will probably be more inf points + inf_is_valid: true inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.55 always_send_full_costmap: True -map_server: - ros__parameters: - # Overridden in launch by the "map" launch configuration or provided default value. - # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" map_saver: ros__parameters: @@ -243,8 +257,10 @@ smoother_server: behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: @@ -257,7 +273,8 @@ behavior_server: plugin: "nav2_behaviors/Wait" assisted_teleop: plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom + local_frame: odom + global_frame: map robot_base_frame: base_link transform_tolerance: 0.1 simulate_ahead_time: 2.0 @@ -278,7 +295,6 @@ waypoint_follower: velocity_smoother: ros__parameters: smoothing_frequency: 20.0 - log_level: debug scale_velocities: False feedback: "OPEN_LOOP" max_velocity: [0.26, 0.0, 1.0] @@ -288,4 +304,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 0.5 + velocity_timeout: 1.0 From 179d5c5a12f4e9dec7f0b880ca5c85639de88109 Mon Sep 17 00:00:00 2001 From: root Date: Mon, 14 Aug 2023 02:39:18 +0000 Subject: [PATCH 61/70] WIP: update tests --- .../nav2_simple_commander/robot_navigator.py | 25 +++++++- .../src/gps_navigation/tester.py | 57 +++++++++++++++---- 2 files changed, 71 insertions(+), 11 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index d5daa3cb09..6bbb1135df 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -25,7 +25,7 @@ from lifecycle_msgs.srv import GetState from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, NavigateThroughPoses, NavigateToPose from nav2_msgs.action import SmoothPath from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes @@ -67,6 +67,7 @@ def __init__(self, node_name='basic_navigator'): 'navigate_through_poses') self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') + self.follow_gps_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_gps_waypoints') self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, 'compute_path_to_pose') @@ -181,6 +182,28 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True + + def followGpsWaypoints(self, gps_poses): + """Send a `FollowGPSWaypoints` action request.""" + self.debug("Waiting for 'FollowWaypoints' action server") + while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowWaypoints' action server not available, waiting...") + + goal_msg = FollowGPSWaypoints.Goal() + goal_msg.poses = gps_poses + + self.info(f'Following {len(goal_msg.gps_poses)} gps goals....') + send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error(f'Following {len(gps_poses)} gps waypoints request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True def spin(self, spin_dist=1.57, time_allowance=10): self.debug("Waiting for 'Spin' action server") diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 77e54fc8da..73b06223c1 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -18,15 +18,17 @@ from action_msgs.msg import GoalStatus from geographic_msgs.msg import GeoPose -from nav2_msgs.action import FollowGPSWaypoints +from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints from nav2_msgs.srv import ManageLifecycleNodes +from rcl_interfaces.srv import SetParameters +from rclpy.parameter import Parameter import rclpy from rclpy.action import ActionClient from rclpy.node import Node -class WaypointFollowerTest(Node): +class GpsWaypointFollowerTest(Node): def __init__(self): super().__init__(node_name="nav2_gps_waypoint_tester", namespace="") self.waypoints = None @@ -35,6 +37,9 @@ def __init__(self): ) self.goal_handle = None + self.param_cli = self.create_client(SetParameters, + '/waypoint_follower/set_parameters') + def setWaypoints(self, waypoints): self.waypoints = [] for wp in waypoints: @@ -44,10 +49,10 @@ def setWaypoints(self, waypoints): msg.orientation.w = 1.0 self.waypoints.append(msg) - def run(self, block): - if not self.waypoints: - rclpy.error_msg("Did not set valid waypoints before running test!") - return False + def run(self, block, cancel): + # if not self.waypoints: + # rclpy.error_msg("Did not set valid waypoints before running test!") + # return False while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'follow_waypoints' action server not available, waiting...") @@ -72,6 +77,9 @@ def run(self, block): return True get_result_future = self.goal_handle.get_result_async() + if cancel: + time.sleep(2) + self.cancel_goal() self.info_msg("Waiting for 'follow_waypoints' action to complete") try: @@ -93,6 +101,13 @@ def run(self, block): self.info_msg("Goal succeeded!") return True + + def setStopFailureParam(self, value): + req = SetParameters.Request() + req.parameters = [Parameter('stop_on_failure', + Parameter.Type.BOOL, value).to_parameter_msg()] + future = self.param_cli.call_async(req) + rclpy.spin_until_future_complete(self, future) def shutdown(self): self.info_msg("Shutting down") @@ -138,20 +153,20 @@ def main(argv=sys.argv[1:]): wps = [[55.944831, -3.186998], [55.944818, -3.187075], [55.944782, -3.187060]] - test = WaypointFollowerTest() + test = GpsWaypointFollowerTest() test.setWaypoints(wps) # wait for poseCallback - result = test.run(True) + result = test.run(True, False) assert result # preempt with new point test.setWaypoints([wps[0]]) - result = test.run(False) + result = test.run(False, False) time.sleep(2) test.setWaypoints([wps[1]]) - result = test.run(False) + result = test.run(False, False) # cancel time.sleep(2) @@ -163,6 +178,28 @@ def main(argv=sys.argv[1:]): result = test.run(True) assert not result result = not result + assert test.action_result.missed_waypoints[0].error_code == \ + ComputePathToPose.Result().GOAL_OUTSIDE_MAP + + # stop on failure test with bogous waypoint + test.setStopFailureParam(True) + bwps = [[55.944831, -3.186998], [35.0, -118.0], [55.944782, -3.187060]] + result = test.run(True, False) + assert not result + result = not result + mwps = test.action_result.missed_waypoints + result = (len(mwps) == 1) & (mwps[0] == 1) + test.setStopFailureParam(False) + + # Zero goal test + test.setWaypoints([]) + result = test.run(True, False) + + # Cancel test + test.setWaypoints(wps) + result = test.run(True, True) + assert not result + result = not result test.shutdown() test.info_msg("Done Shutting Down.") From 0f7c7001995521e17034bbe3d3c34bf78930c92a Mon Sep 17 00:00:00 2001 From: root Date: Tue, 15 Aug 2023 00:54:12 +0000 Subject: [PATCH 62/70] fix tests --- .../src/gps_navigation/tester.py | 7 ++--- .../waypoint_follower.hpp | 6 ++--- .../src/waypoint_follower.cpp | 26 ++++++++----------- 3 files changed, 17 insertions(+), 22 deletions(-) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 73b06223c1..def61c1b2b 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -55,7 +55,7 @@ def run(self, block, cancel): # return False while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'follow_waypoints' action server not available, waiting...") + self.info_msg("'follow_gps_waypoints' action server not available, waiting...") action_request = FollowGPSWaypoints.Goal() action_request.gps_poses = self.waypoints @@ -81,7 +81,7 @@ def run(self, block, cancel): time.sleep(2) self.cancel_goal() - self.info_msg("Waiting for 'follow_waypoints' action to complete") + self.info_msg("Waiting for 'follow_gps_waypoints' action to complete") try: rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status @@ -113,7 +113,7 @@ def shutdown(self): self.info_msg("Shutting down") self.action_client.destroy() - self.info_msg("Destroyed follow_waypoints action client") + self.info_msg("Destroyed follow_gps_waypoints action client") transition_service = "lifecycle_manager_navigation/manage_nodes" mgr_client = self.create_client(ManageLifecycleNodes, transition_service) @@ -184,6 +184,7 @@ def main(argv=sys.argv[1:]): # stop on failure test with bogous waypoint test.setStopFailureParam(True) bwps = [[55.944831, -3.186998], [35.0, -118.0], [55.944782, -3.187060]] + test.setWaypoints(bwps) result = test.run(True, False) assert not result result = not result diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index b8fc071f0a..260e71a37a 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -153,15 +153,13 @@ class WaypointFollower : public nav2_util::LifecycleNode * @brief Action client result callback * @param result Result of action server updated asynchronously */ - template - void resultCallback(const T & result); + void resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); /** * @brief Action client goal response callback * @param goal Response of action server updated asynchronously */ - template - void goalResponseCallback(const T & goal); + void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); /** * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 10f241c900..a86f5e937d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -263,16 +263,12 @@ void WaypointFollower::followWaypointsHandler( client_goal.pose.header.stamp = this->now(); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - std::bind( - &WaypointFollower::resultCallback::WrappedResult>, - this, - std::placeholders::_1); - send_goal_options.goal_response_callback = - std::bind( - &WaypointFollower::goalResponseCallback - ::SharedPtr>, this, + send_goal_options.result_callback = std::bind( + &WaypointFollower::resultCallback, this, std::placeholders::_1); + send_goal_options.goal_response_callback = std::bind( + &WaypointFollower::goalResponseCallback, + this, std::placeholders::_1); future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); @@ -389,9 +385,9 @@ void WaypointFollower::followGPSWaypointsCallback() feedback, result); } -template -void WaypointFollower::resultCallback( - const T & result) +void +WaypointFollower::resultCallback( + const rclcpp_action::ClientGoalHandle::WrappedResult & result) { if (result.goal_id != future_goal_handle_.get()->get_goal_id()) { RCLCPP_DEBUG( @@ -418,9 +414,9 @@ void WaypointFollower::resultCallback( } } -template -void WaypointFollower::goalResponseCallback( - const T & goal) +void +WaypointFollower::goalResponseCallback( + const rclcpp_action::ClientGoalHandle::SharedPtr & goal) { if (!goal) { RCLCPP_ERROR( From 67aec1813157fedd59b77995123e7c4bb5fcb52d Mon Sep 17 00:00:00 2001 From: root Date: Tue, 15 Aug 2023 01:36:42 +0000 Subject: [PATCH 63/70] fixes to nav2 simple commander --- .../nav2_simple_commander/robot_navigator.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 6bbb1135df..54640242d2 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -67,7 +67,7 @@ def __init__(self, node_name='basic_navigator'): 'navigate_through_poses') self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') - self.follow_gps_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_gps_waypoints') + self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints, 'follow_gps_waypoints') self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, 'compute_path_to_pose') @@ -186,14 +186,14 @@ def followWaypoints(self, poses): def followGpsWaypoints(self, gps_poses): """Send a `FollowGPSWaypoints` action request.""" self.debug("Waiting for 'FollowWaypoints' action server") - while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0): + while not self.follow_gps_waypoints_client.wait_for_server(timeout_sec=1.0): self.info("'FollowWaypoints' action server not available, waiting...") goal_msg = FollowGPSWaypoints.Goal() - goal_msg.poses = gps_poses + goal_msg.gps_poses = gps_poses self.info(f'Following {len(goal_msg.gps_poses)} gps goals....') - send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg, + send_goal_future = self.follow_gps_waypoints_client.send_goal_async(goal_msg, self._feedbackCallback) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() From d31cc5349285fafa46cfbd18671bfa40b084b9a8 Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 21 Aug 2023 19:31:09 +0000 Subject: [PATCH 64/70] add robot_localization localizer --- nav2_simple_commander/nav2_simple_commander/robot_navigator.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 54640242d2..2e1f37010f 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -333,7 +333,8 @@ def getResult(self): def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - self._waitForNodeToActivate(localizer) + if localizer != "robot_localization": + self._waitForNodeToActivate(localizer) if localizer == 'amcl': self._waitForInitialPose() self._waitForNodeToActivate(navigator) From 6fe265a8766b2229e4f0e41b9216cc5c0d153c52 Mon Sep 17 00:00:00 2001 From: pepisg Date: Sun, 10 Sep 2023 16:10:58 +0000 Subject: [PATCH 65/70] Bring back from LL client --- nav2_waypoint_follower/src/waypoint_follower.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 4e67822039..a08305cd20 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -94,6 +94,12 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) this), nullptr, std::chrono::milliseconds( 500), false, server_options); + from_ll_to_map_client_ = std::make_unique< + nav2_util::ServiceClient>>( + "/fromLL", + node); + gps_action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), From 40bf684271a335351bd81855cb6f0db93e731294 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Alejandro=20Gonz=C3=A1lez?= <71234974+pepisg@users.noreply.github.com> Date: Sat, 16 Sep 2023 15:22:45 -0500 Subject: [PATCH 66/70] Update nav2_simple_commander/nav2_simple_commander/robot_navigator.py Co-authored-by: Steve Macenski --- nav2_simple_commander/nav2_simple_commander/robot_navigator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 2e1f37010f..dddf9041f2 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -333,7 +333,7 @@ def getResult(self): def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - if localizer != "robot_localization": + if localizer != "robot_localization": # non-lifecycle node self._waitForNodeToActivate(localizer) if localizer == 'amcl': self._waitForInitialPose() From 0bb50729c5f735330a07da2bdddae2e10cac855d Mon Sep 17 00:00:00 2001 From: pepisg Date: Sat, 16 Sep 2023 20:27:36 +0000 Subject: [PATCH 67/70] missing argument in test function --- nav2_system_tests/src/gps_navigation/tester.py | 17 ++++++++++------- .../src/waypoint_follower.cpp | 3 +-- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index def61c1b2b..f0d6f000e5 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -55,7 +55,8 @@ def run(self, block, cancel): # return False while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'follow_gps_waypoints' action server not available, waiting...") + self.info_msg( + "'follow_gps_waypoints' action server not available, waiting...") action_request = FollowGPSWaypoints.Goal() action_request.gps_poses = self.waypoints @@ -101,7 +102,7 @@ def run(self, block, cancel): self.info_msg("Goal succeeded!") return True - + def setStopFailureParam(self, value): req = SetParameters.Request() req.parameters = [Parameter('stop_on_failure', @@ -116,9 +117,11 @@ def shutdown(self): self.info_msg("Destroyed follow_gps_waypoints action client") transition_service = "lifecycle_manager_navigation/manage_nodes" - mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + mgr_client = self.create_client( + ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(f"{transition_service} service not available, waiting...") + self.info_msg( + f"{transition_service} service not available, waiting...") req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -172,14 +175,14 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() - # a failure case + # set waypoint outside of map time.sleep(2) test.setWaypoints([[35.0, -118.0]]) - result = test.run(True) + result = test.run(True, False) assert not result result = not result assert test.action_result.missed_waypoints[0].error_code == \ - ComputePathToPose.Result().GOAL_OUTSIDE_MAP + ComputePathToPose.Result().GOAL_OUTSIDE_MAP # stop on failure test with bogous waypoint test.setStopFailureParam(True) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a08305cd20..920c944ff1 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -39,7 +39,7 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) declare_parameter("action_server_result_timeout", 900.0); - declare_parameter("global_frame_id", global_frame_id_); + declare_parameter("global_frame_id", "map"); nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), @@ -64,7 +64,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); global_frame_id_ = get_parameter("global_frame_id").as_string(); - global_frame_id_ = nav2_util::strip_leading_slash(global_frame_id_); callback_group_ = create_callback_group( From fb3a0354f97a9d81953d14411afaccebb5b32f1f Mon Sep 17 00:00:00 2001 From: pepisg Date: Sat, 16 Sep 2023 23:25:39 +0000 Subject: [PATCH 68/70] small test error --- nav2_system_tests/src/gps_navigation/tester.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index f0d6f000e5..aab8833668 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -36,6 +36,7 @@ def __init__(self): self, FollowGPSWaypoints, "follow_gps_waypoints" ) self.goal_handle = None + self.action_result = None self.param_cli = self.create_client(SetParameters, '/waypoint_follower/set_parameters') @@ -87,6 +88,7 @@ def run(self, block, cancel): rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status result = get_result_future.result().result + self.action_result = result except Exception as e: # noqa: B902 self.error_msg(f"Service call failed {e!r}") From 1ca58db31975b4521d1ede0697f6c97ef620e740 Mon Sep 17 00:00:00 2001 From: pepisg Date: Sun, 17 Sep 2023 00:35:16 +0000 Subject: [PATCH 69/70] style fixes nav2 simple commander --- .../nav2_simple_commander/robot_navigator.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index dddf9041f2..c0c09d80c2 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -25,7 +25,8 @@ from lifecycle_msgs.srv import GetState from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, \ + NavigateThroughPoses, NavigateToPose from nav2_msgs.action import SmoothPath from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes @@ -67,7 +68,8 @@ def __init__(self, node_name='basic_navigator'): 'navigate_through_poses') self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') - self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints, 'follow_gps_waypoints') + self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints, + 'follow_gps_waypoints') self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, 'compute_path_to_pose') @@ -182,7 +184,7 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True - + def followGpsWaypoints(self, gps_poses): """Send a `FollowGPSWaypoints` action request.""" self.debug("Waiting for 'FollowWaypoints' action server") @@ -194,7 +196,7 @@ def followGpsWaypoints(self, gps_poses): self.info(f'Following {len(goal_msg.gps_poses)} gps goals....') send_goal_future = self.follow_gps_waypoints_client.send_goal_async(goal_msg, - self._feedbackCallback) + self._feedbackCallback) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() From a01cbbc08238b9121890d1c24f5092e986070988 Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 20 Sep 2023 02:03:50 +0000 Subject: [PATCH 70/70] rename cartesian action server --- .../nav2_waypoint_follower/waypoint_follower.hpp | 2 +- nav2_waypoint_follower/src/waypoint_follower.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 260e71a37a..8dd0d70cd7 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -198,7 +198,7 @@ class WaypointFollower : public nav2_util::LifecycleNode rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Our action server - std::unique_ptr action_server_; + std::unique_ptr xyz_action_server_; ActionClient::SharedPtr nav_to_pose_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 920c944ff1..f462a771a3 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -83,7 +83,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - action_server_ = std::make_unique( + xyz_action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(), @@ -134,7 +134,7 @@ WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - action_server_->activate(); + xyz_action_server_->activate(); gps_action_server_->activate(); auto node = shared_from_this(); @@ -153,7 +153,7 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - action_server_->deactivate(); + xyz_action_server_->deactivate(); gps_action_server_->deactivate(); dyn_params_handler_.reset(); // destroy bond connection @@ -167,7 +167,7 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - action_server_.reset(); + xyz_action_server_.reset(); nav_to_pose_client_.reset(); gps_action_server_.reset(); from_ll_to_map_client_.reset(); @@ -383,7 +383,7 @@ void WaypointFollower::followWaypointsCallback() followWaypointsHandler, ActionT::Feedback::SharedPtr, ActionT::Result::SharedPtr>( - action_server_, + xyz_action_server_, feedback, result); }