寫在前面
- 當(dāng)前平臺文章匯總地址：ROS2機器人從入門到實戰(zhàn)
- 獲取完整教程及配套資料代碼，請關(guān)注公眾號<魚香ROS>獲取
- 教程配套機器人開發(fā)平臺：兩驅(qū)版| 四驅(qū)版
- 為方便交流，搭建了機器人技術(shù)問答社區(qū)：地址 fishros.org.cn
Navigation 2 對外提供了動作服務(wù)用于導(dǎo)航調(diào)用。動作通信是 ROS 2 四大通訊機制之一，前面我們并沒有介紹，我們以導(dǎo)航調(diào)用為例來簡單了解下它。
動作通信和其名字一樣，主要用于控制場景，它的優(yōu)點在于其反饋機制，當(dāng)客戶端發(fā)送目標(biāo)給服務(wù)端后，除了等待服務(wù)端處理完成，還可以收到服務(wù)端的處理進(jìn)度。啟動導(dǎo)航后在終端中使用動作相關(guān)命令可以查看當(dāng)前系統(tǒng)所有動作列表，命令及結(jié)果如下：
ros2 action list
--
/assisted_teleop
/backup
/compute_path_through_poses
/compute_path_to_pose
/drive_on_heading
/follow_path
/follow_waypoints
/navigate_through_poses
/navigate_to_pose
/smooth_path
/spin
/wait
其中 /navigate_to_pose 就是用于處理導(dǎo)航到點請求的動作。繼續(xù)使用命令查看該動作的具體信息。
ros2 action info /navigate_to_pose -t
---
Action: /navigate_to_pose
Action clients: 4
/bt_navigator [nav2_msgs/action/NavigateToPose]
/waypoint_follower [nav2_msgs/action/NavigateToPose]
/rviz2 [nav2_msgs/action/NavigateToPose]
/rviz2 [nav2_msgs/action/NavigateToPose]
Action servers: 1
/bt_navigator [nav2_msgs/action/NavigateToPose]
可以看到該動作的客戶端、服務(wù)端以及消息接口情況，使用指令查看 nav2_msgs/action/NavigateToPose 接口定義，命令及結(jié)果如下：
ros2 interface show nav2_msgs/action/NavigateToPose
---
#goal definition
geometry_msgs/PoseStamped pose
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
Pose pose
Point position
float64 x
float64 y
float64 z
Quaternion orientation
float64 x 0
float64 y 0
float64 z 0
float64 w 1
string behavior_tree
---
#result definition
std_msgs/Empty result
---
#feedback definition
geometry_msgs/PoseStamped current_pose
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
Pose pose
Point position
float64 x
float64 y
float64 z
Quaternion orientation
float64 x 0
float64 y 0
float64 z 0
float64 w 1
builtin_interfaces/Duration navigation_time
int32 sec
uint32 nanosec
builtin_interfaces/Duration estimated_time_remaining
int32 sec
uint32 nanosec
int16 number_of_recoveries
float32 distance_remaining
從該接口定義可以看出，動作消息的接口分為目標(biāo)、結(jié)果和反饋三個部分，相比服務(wù)通信接口的目標(biāo)和結(jié)果多出了反饋這一部分。如果在機器人導(dǎo)航時仔細(xì)觀察 RViz 左下角 Navigation 2 部分，你會發(fā)現(xiàn)它會實時的顯示當(dāng)前導(dǎo)航所花費的時間，距離目標(biāo)點之間的距離，花費的時間以及脫困次數(shù)，這些數(shù)據(jù)就是來自動作服務(wù)的反饋部分。
使用命令行工具可以發(fā)送動作請求并接收反饋和結(jié)果，以請求機器人移動到地圖的指定目標(biāo)點為例，命令及反饋如下。
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: map}, pose: {position: {x: 2, y: 2}}}}" --feedback
---
Waiting for an action server to become available...
Sending goal:
pose:
header:
stamp:
sec: 0
nanosec: 0
frame_id: map
pose:
position:
x: 2.0
y: 2.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
behavior_tree: ''
Goal accepted with ID: c5c52646de774f55b4ee71ca5ed3267a
...
Feedback:
current_pose:
header:
stamp:
sec: 4679
nanosec: 504000000
frame_id: map
pose:
position:
x: 2.079677095742107
y: 1.947119186243544
z: 0.09189999999999998
orientation:
x: 0.0
y: 0.0
z: 0.054114175706008266
w: 0.998534754521674
navigation_time:
sec: 18
nanosec: 164000000
estimated_time_remaining:
sec: 0
nanosec: 0
number_of_recoveries: 2
distance_remaining: 0.10830961167812347
Result:
result: {}
Goal finished with status: SUCCEEDED
上面的命令用于請求機器人移動到地圖坐標(biāo)系中的 xy 都為 2.0 的點，反饋結(jié)果中，Sending goal 部分表示目標(biāo)，F(xiàn)eedback 部分是反饋，Result 部分為最終結(jié)果。
要靈活使用自然還需要學(xué)習(xí)如何用代碼調(diào)用，在 Python 中 nav2_simple_commander 庫已經(jīng)將動作客戶端代碼封裝到 BasicNavigator 節(jié)點中，使用該節(jié)點的對應(yīng)函數(shù)就可以實現(xiàn)導(dǎo)航操作。在 src/fishbot_application/fishbot_application 下新建文件 nav_to_pose.py，然后編寫如下代碼：
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
def main():
rclpy.init()
navigator = BasicNavigator()
# 等待導(dǎo)航啟動完成
navigator.waitUntilNav2Active()
# 設(shè)置目標(biāo)點坐標(biāo)
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = 1.0
goal_pose.pose.position.y = 1.0
goal_pose.pose.orientation.w = 1.0
# 發(fā)送目標(biāo)接收反饋結(jié)果
navigator.goToPose(goal_pose)
while not navigator.isTaskComplete():
feedback = navigator.getFeedback()
navigator.get_logger().info(
f'預(yù)計: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到達(dá)')
# 超時自動取消
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
navigator.cancelTask()
# 最終結(jié)果判斷
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
navigator.get_logger().info('導(dǎo)航結(jié)果：成功')
elif result == TaskResult.CANCELED:
navigator.get_logger().warn('導(dǎo)航結(jié)果：被取消')
elif result == TaskResult.FAILED:
navigator.get_logger().error('導(dǎo)航結(jié)果：失敗')
else:
navigator.get_logger().error('導(dǎo)航結(jié)果：返回狀態(tài)無效')
上面的代碼中有關(guān)鍵函數(shù)有四個，第一個是 navigator.goToPose 用于發(fā)布目標(biāo)，第二個是 navigator.getFeedback() 用于獲取狀態(tài)反饋，第三個是 navigator.cancelTask() 用于中途取消，第四個是 navigator.getResult() 用于獲取最終結(jié)果。跳轉(zhuǎn)到 goToPose 的源碼可以發(fā)現(xiàn)最終發(fā)送請求是通過 self.nav_to_pose_client.send_goal_async 函數(shù)完成的，而 nav_to_pose_client 就是在 BasicNavigator 函數(shù)中定義的動作客戶端，定義代碼如下：
self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
保存好代碼，注冊該節(jié)點并重新構(gòu)建功能包，再次運行仿真和導(dǎo)航，初始化位姿后啟動該節(jié)點，觀察 RViz 中機器人運動情況，啟動命令及終端打印如下。文章來源：http://www.zghlxwxcb.cn/news/detail-769272.html
ros2 run fishbot_application nav_to_pose
---
[INFO] [1685599886.677153365] [basic_navigator]: Nav2 is ready for use!
[INFO] [1685599886.707280550] [basic_navigator]: Navigating to goal: 1.0 1.0...
[INFO] [1685599886.852565845] [basic_navigator]: 預(yù)計剩余: 0.0 s
[INFO] [1685599891.432155882] [basic_navigator]: 預(yù)計剩余: 170.591972225 s
[INFO] [1685599891.532646643] [basic_navigator]: 預(yù)計剩余: 98.418445514 s
[INFO] [1685599891.635079916] [basic_navigator]: 預(yù)計剩余: 77.541805557 s
[INFO] [1685599914.104374413] [basic_navigator]: 預(yù)計剩余: 1.833780316 s
...
[INFO] [1685599918.156745102] [basic_navigator]: 導(dǎo)航結(jié)果：成功
使用 Python 可以通過調(diào)用 nav2_simple_commander 庫方便的實現(xiàn)導(dǎo)航，但如果項目需要換成 C++ 也并不復(fù)雜，使用動作客戶端也可以方便的調(diào)用，如何實現(xiàn)我并不打算再操作一遍，但我將提供給你一份詳細(xì)的 C++ 調(diào)用導(dǎo)航服務(wù)的實現(xiàn)代碼。文章來源地址http://www.zghlxwxcb.cn/news/detail-769272.html
#include <memory>
#include "nav2_msgs/action/navigate_to_pose.hpp" // 導(dǎo)入導(dǎo)航動作消息的頭文件
#include "rclcpp/rclcpp.hpp" // 導(dǎo)入ROS 2的C++客戶端庫
#include "rclcpp_action/rclcpp_action.hpp" // 導(dǎo)入ROS 2的C++ Action客戶端庫
using NavigationAction = nav2_msgs::action::NavigateToPose; // 定義導(dǎo)航動作類型為NavigateToPose
class NavToPoseClient : public rclcpp::Node {
public:
using NavigationActionClient = rclcpp_action::Client<NavigationAction>; // 定義導(dǎo)航動作客戶端類型
using NavigationActionGoalHandle =
rclcpp_action::ClientGoalHandle<NavigationAction>; // 定義導(dǎo)航動作目標(biāo)句柄類型
NavToPoseClient() : Node("nav_to_pose_client") {
// 創(chuàng)建導(dǎo)航動作客戶端
action_client_ = rclcpp_action::create_client<NavigationAction>(
this, "navigate_to_pose");
}
void sendGoal() {
// 等待導(dǎo)航動作服務(wù)器上線，等待時間為5秒
while (!action_client_->wait_for_action_server(std::chrono::seconds(5))) {
RCLCPP_INFO(get_logger(), "等待Action服務(wù)上線。");
}
// 設(shè)置導(dǎo)航目標(biāo)點
auto goal_msg = NavigationAction::Goal();
goal_msg.pose.header.frame_id = "map"; // 設(shè)置目標(biāo)點的坐標(biāo)系為地圖坐標(biāo)系
goal_msg.pose.pose.position.x = 2.0f; // 設(shè)置目標(biāo)點的x坐標(biāo)為2.0
goal_msg.pose.pose.position.y = 2.0f; // 設(shè)置目標(biāo)點的y坐標(biāo)為2.0
auto send_goal_options =
rclcpp_action::Client<NavigationAction>::SendGoalOptions();
// 設(shè)置請求目標(biāo)結(jié)果回調(diào)函數(shù)
send_goal_options.goal_response_callback =
[this](NavigationActionGoalHandle::SharedPtr goal_handle) {
if (goal_handle) {
RCLCPP_INFO(get_logger(), "目標(biāo)點已被服務(wù)器接收");
}
};
// 設(shè)置移動過程反饋回調(diào)函數(shù)
send_goal_options.feedback_callback =
[this](
NavigationActionGoalHandle::SharedPtr goal_handle,
const std::shared_ptr<const NavigationAction::Feedback> feedback) {
(void)goal_handle; // 假裝調(diào)用，避免 warning: unused
RCLCPP_INFO(this->get_logger(), "反饋剩余距離:%f",
feedback->distance_remaining);
};
// 設(shè)置執(zhí)行結(jié)果回調(diào)函數(shù)
send_goal_options.result_callback =
[this](const NavigationActionGoalHandle::WrappedResult& result) {
if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "處理成功！");
}
};
// 發(fā)送導(dǎo)航目標(biāo)點
action_client_->async_send_goal(goal_msg, send_goal_options);
}
NavigationActionClient::SharedPtr action_client_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<NavToPoseClient>();
node->sendGoal();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
到了這里，關(guān)于【ROS2機器人入門到實戰(zhàn)】使用API進(jìn)行導(dǎo)航的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
