diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 0a2f81c78b9..25551deeb2b 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -301,6 +301,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + transform_tolerance: 0.1 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 0891844443b..b05bb4b5c90 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() @@ -22,6 +23,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" @@ -37,7 +39,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..45a24ae7f3a --- /dev/null +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -0,0 +1,8 @@ +#goal definition +nav2_msgs/OrientedNavSatFix[] gps_poses +--- +#result definition +int32[] missed_waypoints +--- +#feedback +uint32 current_waypoint diff --git a/nav2_msgs/msg/OrientedNavSatFix.msg b/nav2_msgs/msg/OrientedNavSatFix.msg new file mode 100644 index 00000000000..d059e5eae22 --- /dev/null +++ b/nav2_msgs/msg/OrientedNavSatFix.msg @@ -0,0 +1,2 @@ +sensor_msgs/NavSatFix position +geometry_msgs/Quaternion orientation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index c24761cd1aa..73e29a8c0aa 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 diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 9407a6585c5..c0ebbc7618c 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) +# Default to C++17 +set(CMAKE_CXX_STANDARD 17) + # Try for OpenCV 4.X, but settle for whatever is installed find_package(OpenCV 4 QUIET) if(NOT OpenCV_FOUND) @@ -21,6 +24,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() @@ -55,6 +59,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 49589b7dd6b..a42acd48d74 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -25,3 +25,22 @@ 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 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, + +```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, "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/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 1abbdba75b3..be3e29959b7 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -19,17 +19,24 @@ #include #include +#include "rclcpp_action/rclcpp_action.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_msgs/msg/oriented_nav_sat_fix.hpp" #include "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_msgs/action/follow_gps_waypoints.hpp" +#include "nav2_util/service_client.hpp" #include "nav2_core/waypoint_task_executor.hpp" -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" + +#include "robot_localization/srv/from_ll.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/transform_listener.h" namespace nav2_waypoint_follower { @@ -55,6 +62,10 @@ 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 ActionServerGPS = nav2_util::SimpleActionServer; + /** * @brief A constructor for nav2_waypoint_follower::WaypointFollower class */ @@ -98,32 +109,87 @@ class WaypointFollower : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @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 V feedback + * @tparam Z result + * @param action_server + * @param poses + * @param feedback + * @param result + */ + template + void followWaypointsLogic( + const T & action_server, + const V & feedback, + const Z & result); + /** * @brief Action server callbacks */ - void followWaypoints(); + void followWaypointsCallback(); /** - * @brief Action client result callback - * @param result Result of action server updated asynchronously + * @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 resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + 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 */ - void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + template + void goalResponseCallback(const T & goal); + +/** + * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. + * Constructs a vector of stamped poses in map frame and returns them. + * + * @param gps_poses + * @param parent_node + * @param fromll_client + * @return std::vector + */ + std::vector convertGPSPoses2MapPoses( + const std::vector & gps_poses, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node, + const + std::unique_ptr> & fromll_client); + + template + std::vector getLatestGoalPoses(const T & action_server); + + // Common vars used for both GPS and cartesian point following + rclcpp::Node::SharedPtr client_node_; + std::vector failed_ids_; + int loop_rate_; + bool stop_on_failure_; - // Our action server + // Our action server for waypoint following std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; - rclcpp::Node::SharedPtr 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_; + std::unique_ptr> from_ll_to_map_client_; + double transform_tolerance_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader @@ -132,6 +198,10 @@ class WaypointFollower : public nav2_util::LifecycleNode waypoint_task_executor_; std::string waypoint_task_executor_id_; std::string waypoint_task_executor_type_; + + // tf buffer to get transfroms + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/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 3dbb7d21f1e..c791e26d9cd 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("transform_tolerance", 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,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(); + transform_tolerance_ = get_parameter("transform_tolerance").as_double(); std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -72,7 +75,22 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "follow_waypoints", std::bind(&WaypointFollower::followWaypoints, this)); + "follow_waypoints", std::bind(&WaypointFollower::followWaypointsCallback, this)); + + from_ll_to_map_client_ = std::make_unique< + nav2_util::ServiceClient>( + "/fromLL", + client_node_); + + gps_action_server_ = std::make_unique( + get_node_base_interface(), + get_node_clock_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + "follow_gps_waypoints", std::bind(&WaypointFollower::followGPSWaypointsCallback, this)); + // used for transfroming orientation of GPS poses to map frame + tf_buffer_ = std::make_shared(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( @@ -99,6 +117,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 +131,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 +146,8 @@ 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; } @@ -137,25 +159,53 @@ WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void -WaypointFollower::followWaypoints() +template +std::vector WaypointFollower::getLatestGoalPoses( + const T & action_server) { - auto goal = action_server_->get_current_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); + 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 = action_server->get_current_goal()->poses; + } else { + // If GPS waypoint following callback was called, we build here + poses = convertGPSPoses2MapPoses( + action_server->get_current_goal()->gps_poses, shared_from_this(), + from_ll_to_map_client_); + } + return poses; +} + +template +void WaypointFollower::followWaypointsLogic( + const T & action_server, + const V & feedback, + const Z & result) +{ + auto goal = action_server->get_current_goal(); - // Check if request is valid - if (!action_server_ || !action_server_->is_server_active()) { + std::vector poses; + poses = getLatestGoalPoses(action_server); + + 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.empty()) { + RCLCPP_ERROR( + get_logger(), + "Empty vector of waypoints passed to waypoint following " + "action potentially due to conversation failure or empty request." + ); + action_server->terminate_current(result); return; } @@ -165,19 +215,28 @@ 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(client_node_, cancel_future); // for result callback processing spin_some(client_node_); - action_server_->terminate_all(); + 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(); + 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; } @@ -186,20 +245,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]; + client_goal.pose.header.stamp = this->now(); 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); @@ -210,7 +277,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 { @@ -223,7 +290,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!"); @@ -235,7 +302,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 { @@ -250,13 +317,14 @@ WaypointFollower::followWaypoints() { // Update server state goal_index++; + poses[goal_index].header.stamp = this->now(); new_goal = true; - if (goal_index >= goal->poses.size()) { + if (goal_index >= poses.size()) { RCLCPP_INFO( - get_logger(), "Completed all %lu waypoints requested.", - goal->poses.size()); + get_logger(), "Completed all %i waypoints requested.", + static_cast(poses.size())); result->missed_waypoints = failed_ids_; - action_server_->succeeded_current(result); + action_server->succeeded_current(result); failed_ids_.clear(); return; } @@ -266,15 +334,38 @@ WaypointFollower::followWaypoints() (static_cast(now().seconds()) % 30 == 0), "Processing waypoint %i...", goal_index); } - 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, + ActionT::Feedback::SharedPtr, + ActionT::Result::SharedPtr>( + action_server_, + feedback, result); +} + +void WaypointFollower::followGPSWaypointsCallback() +{ + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + followWaypointsLogic, + ActionTGPS::Feedback::SharedPtr, + ActionTGPS::Result::SharedPtr>( + gps_action_server_, + feedback, result); +} + +template +void WaypointFollower::resultCallback( + const T & result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: @@ -292,9 +383,9 @@ WaypointFollower::resultCallback( } } -void -WaypointFollower::goalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal) +template +void WaypointFollower::goalResponseCallback( + const T & goal) { if (!goal) { RCLCPP_ERROR( @@ -304,4 +395,81 @@ WaypointFollower::goalResponseCallback( } } +std::vector +WaypointFollower::convertGPSPoses2MapPoses( + 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.."); + + std::vector poses_in_map_frame_vector; + int waypoint_index = 0; + for (auto && curr_oriented_navsat_fix : gps_poses) { + auto request = std::make_shared(); + auto response = std::make_shared(); + request->ll_point.latitude = curr_oriented_navsat_fix.position.latitude; + request->ll_point.longitude = curr_oriented_navsat_fix.position.longitude; + request->ll_point.altitude = curr_oriented_navsat_fix.position.altitude; + + fromll_client->wait_for_service((std::chrono::seconds(1))); + 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" + "Map frame, going to skip this point!" + "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!", waypoint_index); + return std::vector(); + } + continue; + } else { + // 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 { + utm_to_map_transform = tf_buffer_->lookupTransform("utm", "map", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + parent_node->get_logger(), + "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 = 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; + 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(), + "Converted all %i GPS waypoint to Map frame", + static_cast(poses_in_map_frame_vector.size())); + return poses_in_map_frame_vector; +} + } // namespace nav2_waypoint_follower diff --git a/tools/underlay.repos b/tools/underlay.repos index 73d6b1f1e28..ff4b2d25681 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -23,7 +23,20 @@ repositories: type: git url: https://github.com/ros/bond_core.git version: ros2 + ros/diagnostics: + type: git + url: https://github.com/ros/diagnostics.git + version: ros2-devel + ros/geographic_info: + type: git + url: https://github.com/ros-geographic-info/geographic_info.git + version: ros2 ompl/ompl: type: git url: https://github.com/ompl/ompl.git version: 1.5.0 + robot_localization/robot_localization: + type: git + url: https://github.com/cra-ros-pkg/robot_localization.git + version: foxy-devel + \ No newline at end of file