YOLOのBLOG
ROS 使用 move_base 做4个点循环跑的导航
felicx 化神
  2021 年 03 月 06 日 10:27 2021-03-06 10:27   2021 年 03 月 06 日 10:47      2.9k 字  14 分钟  

前言

记录一下用move_base跑4个目标点的程序。

首先《ros by example 1》里有个用move_base跑4个目标点的程序，先去看了下源码，发现它仅仅是把目标点发送出去，然后等待move_base 60s，如果这段时间内成功完成导航任务，则继续发布下个目标点，否则退出。代码如下所示：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
def move(self, goal):
    # Send the goal pose to the MoveBaseAction server
    self.move_base.send_goal(goal)

    # Allow 1 minute to get there
    finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))

    # If we don't get there in time, abort the goal
    if not finished_within_time:
        self.move_base.cancel_goal()
        rospy.loginfo("Timed out achieving goal")
        else:
            # We made it!
            state = self.move_base.get_state()
            if state == GoalStatus.SUCCEEDED:
                rospy.loginfo("Goal succeeded!")

当时觉得好无语，哪有做控制是靠等待时间来判断的啊，通过当前位姿与目标点位姿的距离才能判断出导航任务是否完成啊。所以，只要把这段代码中添加一个距离的判断就可以实现基于move_base的循环导航了。即：

1
2
if distance < 1.0:
    move_base.send_goal(goal)

只要当前位姿与目标点位姿的距离小于1米了，我就重新发送一个目标点给move_base，这样就可以实现循环导航了（在实验时试过：在导航过程中重新发送目标点，机器人会重新规划处一条到新目标点的路径及轨迹）。那当前时刻的位姿怎么确定呢？有2种方式，一种是订阅定位节点发布的位姿话题；感觉这种有点low，就pass掉了，更优越的方法就是用Actionlib。

Actionlib

为了寻找这种更优的方法，只有去看move_base的源码了！看了代码发现move_base其实是actionlib的服务端的实现，好吧，不知道actionlib是啥。后来又去看actionlib的概念。action也是一种类似于~的问答通讯机制，不一样的地方是action还带有一个反馈机制，可以不断反馈任务的实施进度，而且可以在任务实施过程中，中止运行。哇，带反馈，这不就是我要的更优越的方法么。之后通过下面2位大神的文章以及ROS官网中的教程简单学习了一下actionlib的使用。
ROS探索总结（三十二）——action
ROS知识（15）—-Actionlib的使用（一）
actionlib/ Tutorials

如下图可以看出，action分为服务端和客户端，服务端会不断的向客服端发送反馈信息，而move_base为服务端，上述《ros by example 1 》中的例子为客服端。所以只需要在客户端中使用这个反馈就可以了。那么，怎么实现呢？

image

问题1：是否发出了反馈消息

我们先从第一个问题开始：通过这篇文章，知道了move_base确实是发布了反馈信息，而且反馈信息就是当前的位姿。ok，成功解决第一个问题。注意，这个反馈消息不是move_base发出来的，而是actionlib发出来的！

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
bool MoveBase::executeCycle(
    geometry_msgs::PoseStamped& goal,
    std::vector<geometry_msgs::PoseStamped>& global_plan) {
  //发布速度topic
  geometry_msgs::Twist cmd_vel;

  // push the feedback out
  //发布一些反馈信息
  move_base_msgs::MoveBaseFeedback feedback;
  feedback.base_position = current_position;
  as_->publishFeedback(feedback);

  //省略。。。
}

void MoveBase::executeCb(
    const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal) {
  // we have a goal so start the planner(通知planner线程进行路径规划)
  boost::unique_lock<boost::mutex> lock(planner_mutex_);
  planner_goal_ = goal;
  runPlanner_ = true;
  // 通知规划路径线程
  planner_cond_.notify_one();
  lock.unlock();

  ros::NodeHandle n;
  while (n.ok()) {
    // 被抢占了(可能是发出新的goal，也可能是取消了)
    if (as_->isPreemptRequested()) {
      if (as_->isNewGoalAvailable()) {
        // 发布新的goal，通知planner线程工作。
        planner_cond_.notify_one();
      } else {
        // if we've been preempted explicitly we need to shut things down
        //强制退出
        return;
      }
    }
    //省略。。。
  }
}

与此同时，我也看到了在新的目标点到来时，新的任务抢占了之前的任务，move_base会将新的目标点发送到actionlib中并重新规划路径。

问题2：消息类型

move_base的action通讯的消息类型在安装move_base时会自动安装，ROS中的move_base_msgs提供了这个消息的说明。简单说明一下，消息类型是由2个“—”分隔开的三种数据类型组成。第一条为目标点的消息类型；第二条为当此次动作执行完成时向客户端发送的消息类型，仅在动作结束时发送一次；第三条为反馈的消息类型，这个是在动作执行过程中一直发送的。

在这里可以看到，move_base的反馈消息类型为 geometry_msgs/PoseStamped ，消息的名字为 base_position。

1
2
3
4
5
6
7
8
9
#goal definition
geometry_msgs/PoseStamped target_pose

---
#result definition

---
#feedback
geometry_msgs/PoseStamped base_position

这个反馈消息是发送在/move_base命名空间下的/feedback话题上，也就是/move_base/feedback。通过接收这个话题得到的消息如下所示：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
---
header:
  seq: 640
  stamp:
    secs: 1547635103
    nsecs: 592393429
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1547635097
      nsecs: 258239984
    id: "/nav_test-4-1547635097.258"
  status: 1
  text: "This goal has been accepted by the simple action server"
feedback:
  base_position:
    header:
      seq: 0
      stamp:
        secs: 1547635103
        nsecs: 550548076
      frame_id: "map"
    pose:
      position:
        x: 0.00729128714117
        y: 0.0192558593724
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: -0.0199257022983
        w: 0.999801463486
---

OK，第二个问题成功解决掉。

问题3：怎么接收反馈消息

在学习actionlib时知道了actionlib的使用方式，即，一方为服务端，负责发送动作执行的状态与结果；一方为客户端，负责发送动作目标并监听动作状态。反馈消息是从服务端按照着自己定义的消息类型向客户端发出。所以，反馈消息的处理是在客服端内进行，通过回调函数的方式进行处理。ROS中有例子如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
#include <actionlib/client/simple_action_client.h>
#include <actionlib_tutorials/FibonacciAction.h>
#include <ros/ros.h>

using namespace actionlib_tutorials;
typedef actionlib::SimpleActionClient<FibonacciAction> Client;

// Called once when the goal completes
void doneCb(const actionlib::SimpleClientGoalState& state,
            const FibonacciResultConstPtr& result) {
  ROS_INFO("Finished in state [%s]", state.toString().c_str());
  ROS_INFO("Answer: %i", result->sequence.back());
  ros::shutdown();
}

// Called once when the goal becomes active
void activeCb() { ROS_INFO("Goal just went active"); }

// Called every time feedback is received for the goal
void feedbackCb(const FibonacciFeedbackConstPtr& feedback) {
  ROS_INFO("Got Feedback of length %lu", feedback->sequence.size());
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "test_fibonacci_callback");

  // Create the action client
  Client ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  ac.waitForServer();
  ROS_INFO("Action server started, sending goal.");

  // Send Goal
  FibonacciGoal goal;
  goal.order = 20;
  ac.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

  ros::spin();
  return 0;
}

可以看到，通过在发送目标点时将回调函数注册进去，从而在每次反馈消息到来时将自动调用回调函数，也就是上文中的feedbackCb()函数。在actionlib的源码中可以找到sendGoal()的声明，它为后面三个回调函数给了默认的初始值，一个指向空函数的函数指针。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
/**
 * \brief Sends a goal to the ActionServer, and also registers callbacks
 *
 * If a previous goal is already active when this is called. We simply forget
 * about that goal and start tracking the new goal. No cancel requests are made.
 * \param done_cb     Callback that gets called on transitions to Done
 * \param active_cb   Callback that gets called on transitions to Active
 * \param feedback_cb Callback that gets called whenever feedback for this goal
 * is received
 */
void sendGoal(const Goal& goal,
              SimpleDoneCallback done_cb = SimpleDoneCallback(),
              SimpleActiveCallback active_cb = SimpleActiveCallback(),
              SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());

知道了回调函数的使用方式就可以接受反馈消息了。不完整的几行代码如下：在下面的代码中，每次反馈消息的到来都会调用feedbackCb()，并将base_position赋值到全局变量current_point当中。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> Client;

geometry_msgs::Point current_point;

// Called once when the goal becomes active
void activeCb() { ROS_INFO("Goal Received"); }

// Called every time feedback is received for the goal
void feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback) {
  //	ROS_INFO("Got base_position of Feedback");
  current_point.x = feedback->base_position.pose.position.x;
  current_point.y = feedback->base_position.pose.position.y;
  current_point.z = feedback->base_position.pose.position.z;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "nav_move_base");
  ros::NodeHandle node;

  // Subscribe to the move_base action server
  Client ac("move_base", true);

  // Publisher to manually control the robot (e.g. to stop it, queue_size=5)
  cmdVelPub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 5);

  move_base_msgs::MoveBaseGoal goal;

  // Use the map frame to define goal poses
  goal.target_pose.header.frame_id = "map";

  // Set the time stamp to "now"
  goal.target_pose.header.stamp = ros::Time::now();

  // Set the goal pose to the i-th waypoint
  goal.target_pose.pose = pose_list[count];

  // Start the robot moving toward the goal
  ac.sendGoal(goal, Client::SimpleDoneCallback(), &activeCb, &feedbackCb);

  return 0;
}

可以看到，在sendGoal()函数中，第一个回调函数按照默认的声明给定了空值，并将在客户端中实现的后两个回调函数注册到actionlib中。

1
ac.sendGoal(goal, Client::SimpleDoneCallback(), &activeCb, &feedbackCb);

这个方法使用了全局函数，但是假如我想使用类该怎么做呢？这就需要boost库中的bind了，用法如下：

1
2
3
4
5
actionlib::SimpleActionClient<FibonacciAction> ac;

// Need boost::bind to pass in the 'this' pointer
ac.sendGoal(goal, boost::bind(&MyNode::doneCb, this, _1, _2),
            Client::SimpleActiveCallback(), Client::SimpleFeedbackCallback());

OK，至此，我们已经成功解决上述3个问题并且成功的接受到了作为反馈消息的当前位姿。有了当前位姿我们就可以做很多事情了，包括编写一个基于move_base的循环跑的小程序。

完整程序如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#!/usr/bin/env python

import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from visualization_msgs.msg import Marker
from math import radians, pi


class MoveBaseSquare():
    def __init__(self):
        rospy.init_node('nav_test', anonymous=False)

        rospy.on_shutdown(self.shutdown)

        # How big is the square we want the robot to navigate?
        square_size = rospy.get_param("~square_size", 1.0)  # meters

        # Create a list to hold the target quaternions (orientations)
        quaternions = list()

        # First define the corner orientations as Euler angles
        euler_angles = (pi/2, pi, 3*pi/2, 0)

        # Then convert the angles to quaternions
        for angle in euler_angles:
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            q = Quaternion(*q_angle)
            quaternions.append(q)

        # Create a list to hold the waypoint poses
        waypoints = list()

        # Append each of the four waypoints to the list.  Each waypoint
        # is a pose consisting of a position and orientation in the map frame.
        waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
        waypoints.append(
            Pose(Point(square_size, square_size, 0.0), quaternions[1]))
        waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))
        waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))

        # Initialize the visualization markers for RViz
        self.init_markers()

        # Set a visualization marker at each waypoint
        for waypoint in waypoints:
            p = Point()
            p = waypoint.position
            self.markers.points.append(p)

        # Publisher to manually control the robot (e.g. to stop it, queue_size=5)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient(
            "move_base", MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))

        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting navigation test")

        # Initialize a counter to track waypoints
        i = 0

        # Cycle through the four waypoints
        while i < 4 and not rospy.is_shutdown():
            # Update the marker display
            self.marker_pub.publish(self.markers)

            # Intialize the waypoint goal
            goal = MoveBaseGoal()

            # Use the map frame to define goal poses
            goal.target_pose.header.frame_id = 'map'

            # Set the time stamp to "now"
            goal.target_pose.header.stamp = rospy.Time.now()

            # Set the goal pose to the i-th waypoint
            goal.target_pose.pose = waypoints[i]

            # Start the robot moving toward the goal
            self.move(goal)

            i += 1

    def move(self, goal):
            # Send the goal pose to the MoveBaseAction server
        self.move_base.send_goal(goal)

        # Allow 1 minute to get there
        finished_within_time = self.move_base.wait_for_result(
            rospy.Duration(60))

        # If we don't get there in time, abort the goal
        if not finished_within_time:
            self.move_base.cancel_goal()
            rospy.loginfo("Timed out achieving goal")
        else:
            # We made it!
            state = self.move_base.get_state()
            if state == GoalStatus.SUCCEEDED:
                rospy.loginfo("Goal succeeded!")

    def init_markers(self):
        # Set up our waypoint markers
        marker_scale = 0.2
        marker_lifetime = 0  # 0 is forever
        marker_ns = 'waypoints'
        marker_id = 0
        marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}

        # Define a marker publisher.
        self.marker_pub = rospy.Publisher(
            'waypoint_markers', Marker, queue_size=5)

        # Initialize the marker points list.
        self.markers = Marker()
        self.markers.ns = marker_ns
        self.markers.id = marker_id
        self.markers.type = Marker.CUBE_LIST
        self.markers.action = Marker.ADD
        self.markers.lifetime = rospy.Duration(marker_lifetime)
        self.markers.scale.x = marker_scale
        self.markers.scale.y = marker_scale
        self.markers.color.r = marker_color['r']
        self.markers.color.g = marker_color['g']
        self.markers.color.b = marker_color['b']
        self.markers.color.a = marker_color['a']

        self.markers.header.frame_id = 'odom'
        self.markers.header.stamp = rospy.Time.now()
        self.markers.points = list()

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        # Cancel any active goals
        self.move_base.cancel_goal()
        rospy.sleep(2)
        # Stop the robot
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)


if __name__ == '__main__':
    try:
        MoveBaseSquare()
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation test finished.")

ROS 使用 move_base 做4个点循环跑的导航
2024475831.html
作者
felicx
发布于
2021-03-06 10:27
许可
 评论
评论插件加载失败
正在加载评论插件
  1. 前言
  2. Actionlib
  3. 问题1：是否发出了反馈消息
  4. 问题2：消息类型
  5. 问题3：怎么接收反馈消息
© 2018 - 2024    felicx
由 Hexo 驱动 & 主题 Keep
访客数 访问量
  1. 前言
  2. Actionlib
  3. 问题1：是否发出了反馈消息
  4. 问题2：消息类型
  5. 问题3：怎么接收反馈消息