简单案例-让机器人自己移动
Kong Liangqian Lv6
 2021-04-25 18:38:29  

建立一个工作包

1
2
cd ~/dev_ws/src
ros2 pkg create robot_auto_move --build-type ament_python

创建执行节点

创立两个py节点文件到robot_auto_move文件夹下

1
2
3
cd ~/dev_ws/src
cd robot_auto_move/robot_auto_move
vi robot_estimator.py

robot_estimator.py用于接收机器人目前的位置在哪，此信息是通过odom的topic发布过来，可以启动带有world的launch文件之后查看topic

查看gazebo topic

首先打开启动带有world的launch文件

1
ros2 launch robot_description gazebo_lab_world.launch.py

打开另外一个终端

1
ros2 topic list -t

应该返回

1
2
3
4
5
6
7
8
9
10
11
/clock [rosgraph_msgs/msg/Clock]
/cmd_vel [geometry_msgs/msg/Twist]
/joint_states [sensor_msgs/msg/JointState]
/odom [nav_msgs/msg/Odometry]
/parameter_events [rcl_interfaces/msg/ParameterEvent]
/performance_metrics [gazebo_msgs/msg/PerformanceMetrics]
/robot_description [std_msgs/msg/String]
/rosout [rcl_interfaces/msg/Log]
/scan [sensor_msgs/msg/LaserScan]
/tf [tf2_msgs/msg/TFMessage]
/tf_static [tf2_msgs/msg/TFMessage]

其中/scan是我们通过雷达传感器插件进行发布的，类型为sensor_msgs/msg/LaserScan

查看topic数据类型

首先查看关于雷达信息sensor_msgs/msg/LaserScan的数据结构

1
ros2 inteface show sensor_msgs/msg/LaserScan

返回

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
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

std_msgs/Header header # timestamp in the header is the acquisition time of
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id
                             # the first ray in the scan.
                             #
                             # in frame frame_id, angles are measured around
                             # the positive Z axis (counterclockwise, if Z is up)
                             # with zero angle being forward along the x axis

float32 angle_min            # start angle of the scan [rad]
float32 angle_max            # end angle of the scan [rad]
float32 angle_increment      # angular distance between measurements [rad]

float32 time_increment       # time between measurements [seconds] - if your scanner
                             # is moving, this will be used in interpolating position
                             # of 3d points
float32 scan_time            # time between scans [seconds]

float32 range_min            # minimum range value [m]
float32 range_max            # maximum range value [m]

float32[] ranges             # range data [m]
                             # (Note: values < range_min or > range_max should be discarded)
float32[] intensities        # intensity data [device-specific units].  If your
                             # device does not provide intensities, please leave
                             # the array empty. 

这里包含着许多的数据，包括最小、最大的角度，最大、最小的距离值等等，在此我们主要使用range来做一个简单的判断。注意：在设定雷达信息的时候我们就给了水平距离是接收180个点

查看cmd_vel，即geometry_msgs/msg/Twist的数据结构

1
ros2 interface show geometry_msgs/msg/Twist

返回的信息我们应该熟悉

1
2
3
4
5
6
7
8
9
10
# This expresses velocity in free space broken into its linear and angular parts.

Vector3  linear
	float64 x
	float64 y
	float64 z
Vector3  angular
	float64 x
	float64 y
	float64 z

有两个变量，类型都是Vector3

创建一个运行节点

思路：通过接收的雷达信息，来判断和墙面的距离，从而给出相应的操作。

因此，我们需要一个接收雷达信息的订阅者，以及用于发布速度指令的一个发布者

创建订阅、发布

此订阅者用于接收odom的topic信息，其数据类型是show nav_msgs/msg/Odometry

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
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist                                                                                                                 
from sensor_msgs.msg import LaserScan
from rclpy.qos import qos_profile_sensor_data


class RobotMove(Node):

    def __init__(self):
        super().__init__("RobotMove")
        # 建立一个订阅雷达信息的订阅者
        self.subscribe_ = self.create_subscription(LaserScan, 'scan', self.scan_callback, qos_profile_sensor_data)
		# 建立一个发布速度指令的发布者
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        # 表示达到了改变指令的最小距离
        self.dist_thresh = 0.25
        # 旋转速度
        self.turning_speed = 0.1
        # 移动速度
        self.forward_speed = -0.025


    def scan_callback(self, msg):
        self.leftfront_dist = msg.ranges[180]
        self.front_dist = msg.ranges[90]
        self.rightfront_dist = msg.ranges[0]
        # 一旦接收到了以此scan，就计算如何移动
        self.move()

    def move(self):
        msg = Twist()
        msg.linear.x = 0.0 
        msg.linear.y = 0.0 
        msg.linear.z = 0.0 
        msg.angular.x = 0.0 
        msg.angular.y = 0.0 
        msg.angular.z = 0.0 
    
        d = self.dist_thresh
 		# 周围没有墙面，则前进
    	if self.leftfront_dist > d and self.front_dist > d and self.rightfront_dist > d:
            msg.linear.x = self.forward_speed
		# 右边有墙，左转
        elif self.leftfront_dist > d and  self.rightfront_dist < d:
            msg.angular.z = self.turning_speed
		# 左边有墙，右转
        elif self.leftfront_dist < d  and self.rightfront_dist > d:
            msg.angular.z = -self.turning_speed
		# 其他情况，左转。适用于饶围墙走
        else:
            msg.angular.z = self.turning_speed

		# 发布速度指令
        self.publisher_.publish(msg)

def main():

    # Initialize rclpy library
    rclpy.init(args=args)

    # Create the node
    robot_move = RobotMove()

    # Spin the node so the callback function is called
    # Pull messages from any topics this node is subscribed to
    # Publish any pending messages to the topics
    rclpy.spin(robot_move)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    robot_move.destroy_node()

    # Shutdown the ROS client library for Python
    rclpy.shutdown()

代码审查

1
2
3
4
5
6
7
8
9
10
11
12
13
14
class RobotMove(Node):

    def __init__(self):
        super().__init__("RobotMove")
        # 建立一个订阅雷达信息的订阅者
        self.subscribe_ = self.create_subscription(LaserScan, 'scan', self.scan_callback, qos_profile_sensor_data)
		# 建立一个发布速度指令的发布者
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        # 表示达到了改变指令的最小距离
        self.dist_thresh = 0.25
        # 旋转速度
        self.turning_speed = 0.1
        # 移动速度
        self.forward_speed = 0.1

定义一个接收雷达信息的订阅者self.subscribe_以及一个self.publisher_用于发布速度指令方向，也包括旋转的信息。并定义了订阅者收到消息之后调用的回调函数为self.scan_callback.

  • self.dist_thresh: 用于判断雷达和墙面的距离的临界点
  • self.turning_speed： 用于机器人在Gazebo中旋转的转速
  • self.forward：用于机器人在Gazebo中直行的速度
1
2
3
4
5
6
def scan_callback(self, msg):
    self.leftfront_dist = msg.ranges[180]
    self.front_dist = msg.ranges[90]
    self.rightfront_dist = msg.ranges[0]
    # 一旦接收到了以此scan，就计算如何移动
    self.move()

这是scan订阅者的回调函数，msg为收到的数据，里面的数据已经在之前可以看见。我们简单的获取三个点，即最左右的点和中间的点

然后调用move函数，move是根据当前雷达返回的数据来判断如何发布机器人指令

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
  def move(self):
      msg = Twist()
      msg.linear.x = 0.0 
      msg.linear.y = 0.0 
      msg.linear.z = 0.0 
      msg.angular.x = 0.0 
      msg.angular.y = 0.0 
      msg.angular.z = 0.0 
  
      d = self.dist_thresh
	# 周围没有墙面，则前进
  	if self.leftfront_dist > d and self.front_dist > d and self.rightfront_dist > d:
          msg.linear.x = self.forward_speed
# 右边有墙，左转
      elif self.leftfront_dist > d and  self.rightfront_dist < d:
          msg.angular.z = -self.turning_speed
# 左边有墙，右转
      elif self.leftfront_dist < d  and self.rightfront_dist > d:
          msg.angular.z = self.turning_speed
# 其他情况，右转。适用于饶围墙走
      else:
          msg.angular.z = -self.turning_speed

# 发布速度指令
      self.publisher_.publish(msg)

上面我们写了一个非常简单的move函数，为了方便在每一个if语句里面设定所要下达的命令，在每一次move的时候都创立一个对象

1
2
if self.leftfront_dist > d and self.front_dist > d and self.rightfront_dist > d:
       msg.linear.x = self.forward_speed

第一个if表示左前方和右前方和正前方都没有墙面，则直走

1
2
3
4
5
6
# 右边有墙，左转
      elif self.leftfront_dist > d and  self.rightfront_dist < d:
          msg.angular.z = -self.turning_speed
# 左边有墙，右转
      elif self.leftfront_dist < d  and self.rightfront_dist > d:
          msg.angular.z = self.turning_speed

这里判断左右边是否有墙，则向反向转弯

其余情况，譬如三个方向的距离都小于d，无论什么情况，我们简单处理为左转。

下面是main函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
def main():

    # Initialize rclpy library
    rclpy.init(args=args)

    # Create the node
    robot_move = RobotMove()

    # Spin the node so the callback function is called
    # Pull messages from any topics this node is subscribed to
    # Publish any pending messages to the topics
    rclpy.spin(robot_move)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    robot_move.destroy_node()

    # Shutdown the ROS client library for Python
    rclpy.shutdown()

修改setpy.py

打开setup.py，修改entry_points

1
2
3
4
entry_points={
    'console_scripts': [                                                                                                                        
        'robot_auto_move = robot_auto_move.robot_auto_move_demo:main',
    ],

编译运行

1
2
cd ~/dev_ws
colcon build --packages-select robot_auto_move --symlink-install

仿真运行

打开一个终端，记得source

1
ros2 launch robot_description gazebo_lab_world.launch.py

打开另外一个终端

1
ros2 run robot_auto_move robot_auto_move

打开另外一个终端，打开robot_description下的rviz配置

1
rviz2 rviz.rviz

 Comments