From 5f33ac55ee7bfa6f6e1cc294e500a720245feb69 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Tue, 1 Dec 2020 12:48:37 +0800 Subject: [PATCH 01/28] 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 00000000000..1b7bb2329d3 --- /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 00000000000..ed9bbc99a7b --- /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 00000000000..edaea080059 --- /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 00000000000..cff58bfe8bd --- /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 00000000000..c7efeb22adf --- /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 d09cafa74cf..c770c1364c2 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 00000000000..d83a59170ad --- /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 00265980c7d..e4a908703ca 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/28] 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 c7efeb22adf..05e939cf601 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/28] 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 1b7bb2329d3..00000000000 --- 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 ed9bbc99a7b..00000000000 --- 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 edaea080059..00000000000 --- 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 cff58bfe8bd..00000000000 --- 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 05e939cf601..00000000000 --- 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 3253ab3439e..7f2e2cab584 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 5b8811c357d..b5ad7baf059 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 37b9633ebaf..75866e39a93 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 f4e28a6357a..f16494af58b 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 ff81e7666dd..f4685dcd303 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/28] 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 7f2e2cab584..14d3e1dc4f5 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 b5ad7baf059..66b403bd6ab 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 75866e39a93..e7c50f43e2e 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 f4685dcd303..be85a66b86f 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/28] 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 e7c50f43e2e..6bccb9bd077 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 be85a66b86f..03930051b94 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/28] 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 14d3e1dc4f5..2a43e687518 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 66b403bd6ab..87a469be809 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 6bccb9bd077..79f498cdedc 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 03930051b94..bd589be4014 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/28] 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 79f498cdedc..6642030ffa7 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 bd589be4014..2ac951da029 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 6df91bf25fe..0da86285d8c 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/28] 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 0da86285d8c..0792665d0aa 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/28] 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 0792665d0aa..36af5a99c29 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/28] 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 36af5a99c29..47d9fae14b5 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/28] 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 2ac951da029..4c528c1d32d 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 47d9fae14b5..cc359273876 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/28] 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 cc359273876..ff4b2d25681 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/28] 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 c770c1364c2..99c4c93f674 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 d83a59170ad..45a24ae7f3a 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 00000000000..aa5b024ddbf --- /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 2a43e687518..d3c0afa2242 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 6642030ffa7..3a7a348afac 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 f16494af58b..0ed3197310e 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 4c528c1d32d..583d4cd5a56 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/28] 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 3a7a348afac..c9a823fdab0 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 583d4cd5a56..573a8854ef8 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/28] 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 573a8854ef8..844f7faa3fe 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/28] 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 c9a823fdab0..e1cf89946d8 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 844f7faa3fe..1ba6342ed75 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/28] 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 aa5b024ddbf..d059e5eae22 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 d3c0afa2242..3ca37de2bf8 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 c9a823fdab0..315b6190b62 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 0ed3197310e..f16494af58b 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 844f7faa3fe..8f0f9f4a3df 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/28] 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 880bc2a778b..722d3622b5d 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/28] 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 722d3622b5d..86ab6ffa846 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/28] 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 5b67b12d0d3..5ee79b7fb9f 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 f10edb77292..c62ad7e39e2 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 279f53e8d15..53902392b6b 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 86ab6ffa846..8f66fc7fa24 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/28] 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 5ee79b7fb9f..03272510066 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 c62ad7e39e2..96194909b30 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 53902392b6b..d5962a99d18 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 8f66fc7fa24..8590c31e874 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/28] 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 8590c31e874..30334eaf5c4 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/28] 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 30334eaf5c4..94d6c181ea1 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/28] 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 94d6c181ea1..ba020ff3741 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/28] 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 ba020ff3741..575ed6f3158 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/28] 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 575ed6f3158..a5ee8b8b7b2 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/28] 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 97b66668053..aeb83511252 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 a5ee8b8b7b2..557ffe466d1 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/28] 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 557ffe466d1..c791e26d9cd 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;