小鱼 ROS 2 新书上线!点击链接查看, 新书配套视频点击链接查看。
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
使用Nav2中的waypoint_follower导航,使用的是example_waypoint_follower.py,导航到第一个点之后一直打印 Reached the goal!,但是一直没有走到下一个点
-
使用example_waypoint_follower.py导航到第一个点之后一直打印 Reached the goal!,然后就超时一直重新计算点还是打印Reached the goal!
-
在使用Nav2(Navigation 2)框架中的
waypoint_follower
进行导航时,如果遇到在到达第一个点后一直打印“Reached the goal!”但无法继续移动到下一个点的问题,这通常意味着存在一些配置或逻辑上的问题。以下是一些可能的解决步骤和检查点:-
检查Waypoint List:
- 确保你的waypoint列表(路径点列表)是正确的,并且包含了多个点。
- 检查每个点的位置是否有效,并且它们之间的顺序逻辑正确。
-
检查Goal Setter:
- 确认你的系统中有一个机制(可能是另一个节点或服务)在
waypoint_follower
报告到达当前点后,更新下一个目标点。 - 如果使用的是
example_waypoint_follower.py
,通常需要一个外部逻辑来更新下一个目标点。这可能涉及到订阅某个主题或使用服务来动态更新目标。
- 确认你的系统中有一个机制(可能是另一个节点或服务)在
-
查看日志和调试信息:
- 查看ROS 2的日志输出,特别是
waypoint_follower
节点的输出,看是否有错误或警告信息。 - 检查是否有其他相关节点的错误或异常,这些节点可能影响到目标点的更新或导航的执行。
- 查看ROS 2的日志输出,特别是
-
检查参数配置:
- 检查
waypoint_follower
的配置参数,特别是关于如何接收和处理新的目标点的部分。 - 确保所有的参数都按照你的应用需求进行了正确配置。
- 检查
-
代码和逻辑检查:
- 如果你修改了
example_waypoint_follower.py
或相关的代码,确保逻辑上没有错误。 - 检查是否有任何代码路径可能导致目标点没有被正确更新或处理。
- 如果你修改了
-
仿真或实际环境测试:
- 如果可能,尝试在仿真环境中测试你的导航逻辑,这可以帮助隔离硬件相关的问题。
- 确保地图和环境设置正确,没有导致导航失败的其他外部因素。
-
使用调试工具:
- 使用ROS 2的调试工具,如
ros2 topic echo
和ros2 node list
等,来监视和检查节点的状态和消息流。 - 使用RViz来可视化导航过程,这可以帮助你更直观地理解发生了什么。
- 使用ROS 2的调试工具,如
-
社区和文档:
- 查阅Nav2的官方文档和社区论坛,看看是否有其他人遇到过类似的问题。
- 在必要时,可以在相关的ROS 2社区或论坛中寻求帮助。
通过上述步骤,你应该能够定位并解决
waypoint_follower
在到达第一个点后无法继续导航到下一个点的问题。如果问题依然存在,可能需要更深入地检查你的系统配置和代码逻辑。
以上为生成回答,仅供参考~
-
-
@小伊
waypoint_follower的配置参数如下:
给的实例中的example_waypoint_follower.py如下:
#! /usr/bin/env python3 # Copyright 2021 Samsung Research America # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration """ Basic navigation demo to go to poses. """ def main(): rclpy.init() navigator = BasicNavigator() # Set our demo's initial pose initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' initial_pose.header.stamp = navigator.get_clock().now().to_msg() initial_pose.pose.position.x = 3.45 initial_pose.pose.position.y = 2.15 initial_pose.pose.orientation.z = 1.0 initial_pose.pose.orientation.w = 0.0 navigator.setInitialPose(initial_pose) # Activate navigation, if not autostarted. This should be called after setInitialPose() # or this will initialize at the origin of the map and update the costmap with bogus readings. # If autostart, you should `waitUntilNav2Active()` instead. # navigator.lifecycleStartup() # Wait for navigation to fully activate, since autostarting nav2 navigator.waitUntilNav2Active() # If desired, you can change or load the map as well # navigator.changeMap('/path/to/map.yaml') # You may use the navigator to clear or obtain costmaps # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() # global_costmap = navigator.getGlobalCostmap() # local_costmap = navigator.getLocalCostmap() # set our demo's goal poses to follow goal_poses = [] goal_pose1 = PoseStamped() goal_pose1.header.frame_id = 'map' goal_pose1.header.stamp = navigator.get_clock().now().to_msg() goal_pose1.pose.position.x = 1.5 goal_pose1.pose.position.y = 0.55 goal_pose1.pose.orientation.w = 0.707 goal_pose1.pose.orientation.z = 0.707 goal_poses.append(goal_pose1) # additional goals can be appended goal_pose2 = PoseStamped() goal_pose2.header.frame_id = 'map' goal_pose2.header.stamp = navigator.get_clock().now().to_msg() goal_pose2.pose.position.x = 1.5 goal_pose2.pose.position.y = -3.75 goal_pose2.pose.orientation.w = 0.707 goal_pose2.pose.orientation.z = 0.707 goal_poses.append(goal_pose2) goal_pose3 = PoseStamped() goal_pose3.header.frame_id = 'map' goal_pose3.header.stamp = navigator.get_clock().now().to_msg() goal_pose3.pose.position.x = -3.6 goal_pose3.pose.position.y = -4.75 goal_pose3.pose.orientation.w = 0.707 goal_pose3.pose.orientation.z = 0.707 goal_poses.append(goal_pose3) # sanity check a valid path exists # path = navigator.getPath(initial_pose, goal_pose1) nav_start = navigator.get_clock().now() navigator.followWaypoints(goal_poses) i = 0 while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! # ################################################ # Do something with the feedback i = i + 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: print('Executing current waypoint: ' + str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses))) now = navigator.get_clock().now() # Some navigation timeout to demo cancellation if now - nav_start > Duration(seconds=600.0): navigator.cancelTask() # Some follow waypoints request change to demo preemption if now - nav_start > Duration(seconds=35.0): goal_pose4 = PoseStamped() goal_pose4.header.frame_id = 'map' goal_pose4.header.stamp = now.to_msg() goal_pose4.pose.position.x = -5.0 goal_pose4.pose.position.y = -4.75 goal_pose4.pose.orientation.w = 0.707 goal_pose4.pose.orientation.z = 0.707 goal_poses = [goal_pose4] nav_start = now navigator.followWaypoints(goal_poses) # Do something depending on the return code result = navigator.getResult() if result == TaskResult.SUCCEEDED: print('Goal succeeded!') elif result == TaskResult.CANCELED: print('Goal was canceled!') elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') navigator.lifecycleShutdown() exit(0) if __name__ == '__main__': main()
上述判断只在feedback = navigator.getFeedback()这个当中返回完成情况。但是控制一直打印到达点,feedback = navigator.getFeedback()却一直停留在第一个点没有到下一个点,怀疑是中间传输结果或者通讯有问题
-
在您提供的代码中,如果
feedback = navigator.getFeedback()
一直停留在第一个点没有更新,可能有几个潜在的原因和解决方案。这里是一些可能的排查步骤和考虑因素:-
检查导航器的反馈机制:
- 确保
BasicNavigator
类中的getFeedback
方法能够正确返回当前的导航状态,特别是当前正在前往的waypoint索引。 - 查看是否有日志或调试信息表明导航器在处理完第一个waypoint后是否正确地接收到了下一个waypoint的信息。
- 确保
-
检查时间戳和更新频率:
- 在
while not navigator.isTaskComplete()
循环中,检查时间戳的更新是否足够频繁。如果循环的迭代速度不够快,可能会导致反馈信息的延迟。 - 确保ROS系统中的时间戳是正确同步的,因为
PoseStamped
消息依赖于时间戳来正确解析位置信息。
- 在
-
检查ROS通信:
- 使用ROS工具(如
rostopic list
和rostopic echo
)检查相关的topic(如用于发送goal poses和接收feedback的topic)是否活跃,并且数据是否在预期的时间间隔内更新。 - 检查是否有任何节点因为通信问题(如网络延迟或节点崩溃)而未能正确发布或订阅消息。
- 使用ROS工具(如
-
检查导航算法的实现:
- 如果
BasicNavigator
类是基于某种特定的导航算法(如A*、Dijkstra或某种路径规划算法),请确保该算法能够正确处理多个连续的goal poses。 - 检查是否有逻辑错误或边界条件处理不当,导致在第一个waypoint之后无法继续。
- 如果
-
调试和日志:
- 在
BasicNavigator
类和相关的ROS节点中添加更多的日志输出,以便跟踪导航过程中的状态和事件。 - 使用ROS的日志系统(如
rclpy.logging
)来记录关键步骤和变量值,这有助于诊断问题。
- 在
-
代码和依赖的更新:
- 确保所有相关的ROS包和依赖都是最新的,并且与您的ROS版本兼容。
- 如果可能,尝试在一个干净的ROS环境中重新构建和运行您的代码,以排除由于环境或依赖冲突导致的问题。
-
物理环境检查:
- 如果您的机器人正在物理环境中运行,检查是否有任何物理障碍或环境变化可能导致导航失败。
通过上述步骤,您应该能够更准确地诊断问题所在,并采取相应的措施来解决
feedback
未更新的问题。如果问题仍然存在,可能需要更深入地检查BasicNavigator
类的实现或寻求来自ROS社区的帮助。
以上为生成回答,仅供参考~
-
-
@小伊
我发现使用example_nav_through_poses.py中的goThroughPoses导航也是一样的结果,都是到达第一个点之后,就不动了。
这是example_nav_through_poses.py的日志
-
要解决
example_nav_through_poses.py
脚本在导航到第一个点之后停止的问题,我们需要详细检查几个可能的方面。以下是一些步骤和建议,帮助你排查和解决这个问题:-
检查目标点设置:
确保你设置的导航点是有效且可达的。有时候,如果第一个点设置得太远或者环境中有障碍物阻挡,机器人可能无法到达该点,从而导致导航停止。 -
检查机器人状态:
确认机器人在开始导航前是处于正确的状态,比如没有卡住,电池电量充足,传感器正常工作等。 -
查看日志输出:
详细查看脚本运行时的日志输出,特别是关于导航状态和错误信息的部分。这可能包括路径规划失败、碰撞检测、传感器数据异常等。 -
检查地图和定位:
确保机器人的地图是正确的,并且机器人能够正确地定位自己在地图上的位置。定位错误可能导致导航失败。 -
传感器和障碍物检测:
检查机器人的传感器是否工作正常,特别是激光雷达或深度相机等用于避障的传感器。如果传感器数据不准确,机器人可能会因为误判障碍物而停止。 -
代码审查:
检查example_nav_through_poses.py
脚本中的代码,特别是与导航相关的部分。确保所有的循环和条件判断都按预期工作。特别是检查是否有代码逻辑错误导致导航在第一个点之后停止。 -
依赖库和版本:
确保你使用的ROS(机器人操作系统)版本和所有相关的依赖库都是兼容的。有时候,软件更新可能导致旧的代码不再有效。 -
简化测试:
尝试减少导航点的数量,只设置两个点来看是否能成功导航到第二个点。这有助于确定问题是否出在特定的导航点上。 -
社区和资源:
查看ROS社区论坛或相关文档,看看是否有其他人遇到并解决了类似的问题。 -
调试工具:
使用ROS提供的调试工具,如rqt_console
和rqt_graph
,来监控节点的状态和信息流。
如果以上步骤都不能解决问题,你可能需要更深入地分析机器人的行为,或者考虑是否有硬件故障的可能性。在排查过程中,保持耐心和细致是关键。
以上为生成回答,仅供参考~
-
-
@小鱼 在 提问前必看!不符合要求的问题拒绝回答!! 中说:
- 不要截图,要复制粘贴,一定要代码块包裹
不要截图的原因是,代码方便回答者去复制帮你搜索,而且无论是在手机上还是电脑上都更容易看。
用代码块包裹是为了更加美观,不会一个代码很长一段,不易阅读
- 不要截图,要复制粘贴,一定要代码块包裹
-
@小伊 导航的点都是可以正常到达的,到达第一个点之后控制服务已经打印到达目标点,然后就是返回值给行为树,行为树再返回值给waypoint_follower.cpp,waypoint_follower.cpp收到成功之后才会发送下一个点。
bt_action_node.hpp行为树节点代码如下:
// Copyright (c) 2018 Intel Corporation // // 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_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_ #define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_ #include <memory> #include <string> #include <chrono> #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_behavior_tree/bt_conversions.hpp" namespace nav2_behavior_tree { using namespace std::chrono_literals; // NOLINT /** * @brief Abstract class representing an action based BT node * @tparam ActionT Type of action */ template<class ActionT> class BtActionNode : public BT::ActionNodeBase { public: /** * @brief A nav2_behavior_tree::BtActionNode constructor * @param xml_tag_name Name for the XML tag for this node * @param action_name Action name this node creates a client for * @param conf BT node configuration */ BtActionNode( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true) { node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node"); callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard auto bt_loop_duration = config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration"); server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout"); getInput<std::chrono::milliseconds>("server_timeout", server_timeout_); wait_for_service_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout"); // timeout should be less than bt_loop_duration to be able to finish the current tick max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5); // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult(); std::string remapped_action_name; if (getInput("server_name", remapped_action_name)) { action_name_ = remapped_action_name; } createActionClient(action_name_); // Give the derive class a chance to do any initialization RCLCPP_DEBUG(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str()); } BtActionNode() = delete; virtual ~BtActionNode() { } /** * @brief Create instance of an action client * @param action_name Action name to create client for */ void createActionClient(const std::string & action_name) { // Now that we have the ROS node to use, create the action client for this BT action action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_); // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs", action_name.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available")); } } /** * @brief Any subclass of BtActionNode that accepts parameters must provide a * providedPorts method and call providedBasicPorts in it. * @param addition Additional ports to add to BT port list * @return BT::PortsList Containing basic ports along with node-specific ports */ static BT::PortsList providedBasicPorts(BT::PortsList addition) { BT::PortsList basic = { BT::InputPort<std::string>("server_name", "Action server name"), BT::InputPort<std::chrono::milliseconds>("server_timeout") }; basic.insert(addition.begin(), addition.end()); return basic; } /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports */ static BT::PortsList providedPorts() { return providedBasicPorts({}); } // Derived classes can override any of the following methods to hook into the // processing for the action: on_tick, on_wait_for_result, and on_success /** * @brief Function to perform some user-defined operation on tick * Could do dynamic checks, such as getting updates to values on the blackboard */ virtual void on_tick() { } /** * @brief Function to perform some user-defined operation after a timeout * waiting for a result that hasn't been received yet. Also provides access to * the latest feedback message from the action server. Feedback will be nullptr * in subsequent calls to this function if no new feedback is received while waiting for a result. * @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received */ virtual void on_wait_for_result(std::shared_ptr<const typename ActionT::Feedback>/*feedback*/) { } /** * @brief Function to perform some user-defined operation upon successful * completion of the action. Could put a value on the blackboard. * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value */ virtual BT::NodeStatus on_success() { return BT::NodeStatus::SUCCESS; } /** * @brief Function to perform some user-defined operation when the action is aborted. * @return BT::NodeStatus Returns FAILURE by default, user may override return another value */ virtual BT::NodeStatus on_aborted() { return BT::NodeStatus::FAILURE; } /** * @brief Function to perform some user-defined operation when the action is cancelled. * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value */ virtual BT::NodeStatus on_cancelled() { return BT::NodeStatus::SUCCESS; } /** * @brief The main override required by a BT action * @return BT::NodeStatus Status of tick execution */ BT::NodeStatus tick() override { // first step to be done only at the beginning of the Action if (status() == BT::NodeStatus::IDLE) { // setting the status to RUNNING to notify the BT Loggers (if any) setStatus(BT::NodeStatus::RUNNING); // reset the flag to send the goal or not, allowing the user the option to set it in on_tick should_send_goal_ = true; // user defined callback, may modify "should_send_goal_". on_tick(); if (!should_send_goal_) { return BT::NodeStatus::FAILURE; } send_new_goal(); } try { // if new goal was sent and action server has not yet responded // check the future goal handle if (future_goal_handle_) { auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>(); if (!is_future_goal_handle_complete(elapsed)) { // return RUNNING if there is still some time before timeout happens if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; } // if server has taken more time than the specified timeout value return FAILURE RCLCPP_WARN( node_->get_logger(), "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); future_goal_handle_.reset(); return BT::NodeStatus::FAILURE; } } // The following code corresponds to the "RUNNING" loop if (rclcpp::ok() && !goal_result_available_) { // user defined callback. May modify the value of "goal_updated_" on_wait_for_result(feedback_); // reset feedback to avoid stale information feedback_.reset(); auto goal_status = goal_handle_->get_status(); if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; send_new_goal(); auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>(); if (!is_future_goal_handle_complete(elapsed)) { if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; } RCLCPP_WARN( node_->get_logger(), "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); future_goal_handle_.reset(); return BT::NodeStatus::FAILURE; } } callback_group_executor_.spin_some(); // check if, after invoking spin_some(), we finally received the result if (!goal_result_available_) { // Yield this Action, returning RUNNING return BT::NodeStatus::RUNNING; } } } catch (const std::runtime_error & e) { if (e.what() == std::string("send_goal failed") || e.what() == std::string("Goal was rejected by the action server")) { // Action related failure that should not fail the tree, but the node return BT::NodeStatus::FAILURE; } else { // Internal exception to propagate to the tree throw e; } } BT::NodeStatus status; switch (result_.code) { case rclcpp_action::ResultCode::SUCCEEDED: RCLCPP_ERROR(node_->get_logger(),"rclcpp_action::ResultCode::SUCCEEDED"); status = on_success(); break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(node_->get_logger(),"rclcpp_action::ResultCode::ABORTED"); status = on_aborted(); break; case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(node_->get_logger(),"rclcpp_action::ResultCode::CANCELED"); status = on_cancelled(); break; default: throw std::logic_error("BtActionNode::Tick: invalid status value"); } goal_handle_.reset(); return status; } /** * @brief The other (optional) override required by a BT action. In this case, we * make sure to cancel the ROS2 action if it is still running. */ void halt() override { if (should_cancel_goal()) { auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); } if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), "Failed to get result for %s in node halt!", action_name_.c_str()); } on_cancelled(); } setStatus(BT::NodeStatus::IDLE); } protected: /** * @brief Function to check if current goal should be cancelled * @return bool True if current goal should be cancelled, false otherwise */ bool should_cancel_goal() { // Shut the node down if it is currently running if (status() != BT::NodeStatus::RUNNING) { return false; } // No need to cancel the goal if goal handle is invalid if (!goal_handle_) { return false; } callback_group_executor_.spin_some(); auto status = goal_handle_->get_status(); // Check if the goal is still executing return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; } /** * @brief Function to send new goal to action server */ void send_new_goal() { goal_result_available_ = false; auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions(); send_goal_options.result_callback = [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) { if (future_goal_handle_) { RCLCPP_DEBUG( node_->get_logger(), "Goal result for %s available, but it hasn't received the goal response yet. " "It's probably a goal result for the last goal request", action_name_.c_str()); return; } // TODO(#1652): a work around until rcl_action interface is updated // if goal ids are not matched, the older goal call this callback so ignore the result // if matched, it must be processed (including aborted) if (this->goal_handle_->get_goal_id() == result.goal_id) { goal_result_available_ = true; result_ = result; } }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const typename ActionT::Feedback> feedback) { feedback_ = feedback; }; future_goal_handle_ = std::make_shared< std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>( action_client_->async_send_goal(goal_, send_goal_options)); time_goal_sent_ = node_->now(); } /** * @brief Function to check if the action server acknowledged a new goal * @param elapsed Duration since the last goal was sent and future goal handle has not completed. * After waiting for the future to complete, this value is incremented with the timeout value. * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise */ bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed) { auto remaining = server_timeout_ - elapsed; // server has already timed out, no need to sleep if (remaining <= std::chrono::milliseconds(0)) { future_goal_handle_.reset(); return false; } auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining; auto result = callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout); elapsed += timeout; if (result == rclcpp::FutureReturnCode::INTERRUPTED) { future_goal_handle_.reset(); throw std::runtime_error("send_goal failed"); } if (result == rclcpp::FutureReturnCode::SUCCESS) { goal_handle_ = future_goal_handle_->get(); future_goal_handle_.reset(); if (!goal_handle_) { throw std::runtime_error("Goal was rejected by the action server"); } return true; } return false; } /** * @brief Function to increment recovery count on blackboard if this node wraps a recovery */ void increment_recovery_count() { int recovery_count = 0; config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT recovery_count += 1; config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT } std::string action_name_; typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_; // All ROS2 actions have a goal and a result typename ActionT::Goal goal_; bool goal_updated_{false}; bool goal_result_available_{false}; typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_; typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_; // To handle feedback from action server std::shared_ptr<const typename ActionT::Feedback> feedback_; // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; // The timeout value while waiting for response from a server when a // new action goal is sent or canceled std::chrono::milliseconds server_timeout_; // The timeout value for BT loop execution std::chrono::milliseconds max_timeout_; // The timeout value for waiting for a service to response std::chrono::milliseconds wait_for_service_timeout_; // To track the action server acknowledgement when a new goal is sent std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>> future_goal_handle_; rclcpp::Time time_goal_sent_; // Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent. bool should_send_goal_; }; } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
navigate_to_pose.cpp代码如下:
// Copyright (c) 2021 Samsung Research // // 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 <vector> #include <string> #include <set> #include <memory> #include <limits> #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" namespace nav2_bt_navigator { bool NavigateToPoseNavigator::configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); if (!node->has_parameter("goal_blackboard_id")) { node->declare_parameter("goal_blackboard_id", std::string("goal")); } goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string(); if (!node->has_parameter("path_blackboard_id")) { node->declare_parameter("path_blackboard_id", std::string("path")); } path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; self_client_ = rclcpp_action::create_client<ActionT>(node, getName()); goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>( "goal_pose", rclcpp::SystemDefaultsQoS(), std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1)); return true; } std::string NavigateToPoseNavigator::getDefaultBTFilepath( rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) { std::string default_bt_xml_filename; auto node = parent_node.lock(); if (!node->has_parameter("default_nav_to_pose_bt_xml")) { std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); node->declare_parameter<std::string>( "default_nav_to_pose_bt_xml", pkg_share_dir + "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); } node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename); return default_bt_xml_filename; } bool NavigateToPoseNavigator::cleanup() { goal_sub_.reset(); self_client_.reset(); return true; } bool NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { auto bt_xml_filename = goal->behavior_tree; if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( logger_, "BT file not found: %s. Navigation canceled.", bt_xml_filename.c_str()); return false; } initializeGoalPose(goal); return true; } void NavigateToPoseNavigator::goalCompleted( typename ActionT::Result::SharedPtr /*result*/, const nav2_behavior_tree::BtStatus /*final_bt_status*/) { } void NavigateToPoseNavigator::onLoop() { // action server feedback (pose, duration of task, // number of recoveries, and distance remaining to goal) auto feedback_msg = std::make_shared<ActionT::Feedback>(); geometry_msgs::msg::PoseStamped current_pose; nav2_util::getCurrentPose( current_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.robot_frame, feedback_utils_.transform_tolerance); auto blackboard = bt_action_server_->getBlackboard(); try { // Get current path points nav_msgs::msg::Path current_path; blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = [¤t_pose, ¤t_path]() { size_t closest_pose_idx = 0; double curr_min_dist = std::numeric_limits<double>::max(); for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { double curr_dist = nav2_util::geometry_utils::euclidean_distance( current_pose, current_path.poses[curr_idx]); if (curr_dist < curr_min_dist) { curr_min_dist = curr_dist; closest_pose_idx = curr_idx; } } return closest_pose_idx; }; // Calculate distance on the path double distance_remaining = nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx()); // Default value for time remaining rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0); // Get current speed geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist(); double current_linear_speed = std::hypot(current_odom.linear.x, current_odom.linear.y); // Calculate estimated time taken to goal if speed is higher than 1cm/s // and at least 10cm to go if ((std::abs(current_linear_speed) > 0.01) && (distance_remaining > 0.1)) { estimated_time_remaining = rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed)); } feedback_msg->distance_remaining = distance_remaining; feedback_msg->estimated_time_remaining = estimated_time_remaining; } catch (...) { // Ignore } int recovery_count = 0; blackboard->get<int>("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; bt_action_server_->publishFeedback(feedback_msg); } void NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) { RCLCPP_INFO(logger_, "Received goal preemption request"); if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() || (goal->behavior_tree.empty() && bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename())) { // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file initializeGoalPose(bt_action_server_->acceptPendingGoal()); } else { RCLCPP_WARN( logger_, "Preemption request was rejected since the requested BT XML file is not the same " "as the one that the current goal is executing. Preemption with a new BT is invalid " "since it would require cancellation of the previous goal instead of true preemption." "\nCancel the current goal and send a new action request if you want to use a " "different BT XML file. For now, continuing to track the last goal until completion."); bt_action_server_->terminatePendingGoal(); } } void NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) { geometry_msgs::msg::PoseStamped current_pose; nav2_util::getCurrentPose( current_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.robot_frame, feedback_utils_.transform_tolerance); RCLCPP_INFO( logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)", current_pose.pose.position.x, current_pose.pose.position.y, goal->pose.pose.position.x, goal->pose.pose.position.y); // Reset state for new action feedback start_time_ = clock_->now(); auto blackboard = bt_action_server_->getBlackboard(); blackboard->set<int>("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose); } void NavigateToPoseNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose) { ActionT::Goal goal; goal.pose = *pose; self_client_->async_send_goal(goal); } } // namespace nav2_bt_navigator