ROS笔记（36） 避障运动规划

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接： https://blog.csdn.net/qq_32618327/article/details/100010542

---

1. 避障问题

工作环境中会有一些周围物体，这些物体有可能成为机器人运动规划过程中的障碍物
所以运动规划需要考虑避障问题

Movelt!中默认使用的运动规划库OMPL支持避障规划
可以使用move group中的 planning scene 插件的相关接口加入障碍物模型
并且维护机器人工作的场景信息

先来梳理一下主要流程：

1. 启动机械臂，初始化场景
2. 在可视化环境中加入障碍物模型
3. 设置机器人的起始位姿和目标位姿
4. 进行避障规划

---

2. 启动机械臂

第一步，启动arm_planning.launch文件

$ roslaunch hharm_planning arm_planning.launch

---

3. 添加障碍

剩下的步骤，创建moveit_obstacles_demo.py文件来执行
代码较长，就按一些功能分类来叙述
第二步需要的是添加障碍物

from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import  PlanningScene, ObjectColor

PlanningSceneInterface接口：提供了添加、删除物体模型的功能
PlanningScene消息：场景更新话题planning_scene订阅的消息类型
ObjectColor消息：设置物体的颜色

# 初始化场景对象
scene = PlanningSceneInterface()

# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)

创建一个PlanningSceneInterface类的实例，通过这个实例可以添加或删除物体模型
创建一个planning_scene话题的发布者，用来更新物体颜色等信息

# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'

# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)    

物体模型在场景中需要有唯一的ID，这里包含三个物体：一张桌子和两个盒子
为了可以在终端中重复运行，但之前加载的物体模型并不会自动清除
所以，需要使用 remove_world_object 清除指定的物体模型

# 设置桌面的高度
table_ground = 0.25

# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]

设置桌子的离地高度以及三个物体的模型尺寸
这里都使用长方体描述模型，所以需要宽、高尺寸，单位是米

# 设置三个物体的场景位置并且加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.26
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)

box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.21
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0   
scene.add_box(box1_id, box1_pose, box1_size)

box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.19
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0   
scene.add_box(box2_id, box2_pose, box2_size)

除了物体的尺寸，还需要设置物体在场景中的位置，使用PoseStamped消息描述
确定物体的位置后，使用PlanningSceneInterface的add_box接口将物体添加到场景中
共有三个参数：物体模型的ID、在场景中的位置、物体的尺寸

# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

# 将场景中的颜色设置发布
self.sendColors()    

为了在rviz中更明确显示物体模型，可以将物体设置为不同的颜色：
桌子是红色，两个盒子是橙色
这里用到了两个例程中的函数setColor 和 sendColors

# 设置场景物体的颜色
def setColor(self, name, r, g, b, a = 0.9):
    # 初始化moveit颜色对象
    color = ObjectColor()
    
    # 设置颜色值
    color.id = name
    color.color.r = r
    color.color.g = g
    color.color.b = b
    color.color.a = a
    
    # 更新颜色字典
    self.colors[name] = color

通过输入的RGBA值创建一条ObjectColor类型的消息，并保存到颜色变量列表中

# 将颜色设置发送并应用到moveit场景当中
def sendColors(self):

    # 初始化规划场景对象
    p = PlanningScene()

    # 需要设置规划场景是否有差异     
    p.is_diff = True
    
    # 从颜色字典中取出颜色设置
    for color in self.colors.values():
        p.object_colors.append(color)
    
    # 发布场景物体颜色设置
    self.scene_pub.publish(p)

取出颜色列表中的颜色，通过创建的PlanningScene消息，将场景的更新信息发布
rviz中对应的物体颜色就会发生改变

添加障碍效果如下：

---

4. 设置位姿

场景配置完成后，就可以准备让机器人运动了

# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.2
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0

# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()

为了达到避障效果，先将机器人运动到桌面上的两个盒子之间作为起始位姿

---

5. 避障规划

起始位姿设置完成后，接着设置目标位姿在旁边盒子的外侧，就可以让机器人开始运动了

# 设置机械臂的运动目标位置，进行避障规划
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.pose.position.x = 0.2
target_pose2.pose.position.y = -0.25
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose2.pose.orientation.w = 1.0

# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose2, end_effector_link)
arm.go()

Movelt!规划的运动轨迹会自动避开两个盒子和桌子等障碍物

---

6. 启动避障规划

moveit_obstacles_demo.py文件的源码

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import  PlanningScene, ObjectColor
from geometry_msgs.msg import PoseStamped, Pose

class MoveItObstaclesDemo:
    def __init__(self):

        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化moveit_obstacles_demo节点
        rospy.init_node('moveit_obstacles_demo')
        
        # 初始化场景对象
        scene = PlanningSceneInterface()
        
        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
        
        # 创建一个存储物体颜色的字典对象
        self.colors = dict()
        
        # 等待场景准备就绪
        rospy.sleep(1)
                        
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('arm')
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
       
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
        
        # 设置每次运动规划的时间限制：5s
        arm.set_planning_time(5)
        
        # 设置场景物体的名称
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)    
        rospy.sleep(1)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
        
        # 设置桌面的高度
        table_ground = 0.25
        
        # 设置table、box1和box2的三维尺寸
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # 设置三个物体的场景位置并且加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.26
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.21
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.19
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)
        
        # 将桌子设置成红色，两个box设置成橙色
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # 将场景中的颜色设置发布
        self.sendColors()    
        
        # 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.2
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose.pose.orientation.w = 1.0
        
        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # 设置机械臂的运动目标位置，进行避障规划
        target_pose2 = PoseStamped()
        target_pose2.header.frame_id = reference_frame
        target_pose2.pose.position.x = 0.2
        target_pose2.pose.position.y = -0.25
        target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose2.pose.orientation.w = 1.0
        
        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose2, end_effector_link)
        arm.go()
        rospy.sleep(2)
        
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
        
    # 设置场景物体的颜色
    def setColor(self, name, r, g, b, a = 0.9):
        # 初始化moveit颜色对象
        color = ObjectColor()
        
        # 设置颜色值
        color.id = name
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a
        
        # 更新颜色字典
        self.colors[name] = color

    # 将颜色设置发送并应用到moveit场景当中
    def sendColors(self):

        # 初始化规划场景对象
        p = PlanningScene()

        # 需要设置规划场景是否有差异     
        p.is_diff = True
        
        # 从颜色字典中取出颜色设置
        for color in self.colors.values():
            p.object_colors.append(color)
        
        # 发布场景物体颜色设置
        self.scene_pub.publish(p)

if __name__ == "__main__":
    try:
        MoveItObstaclesDemo()
    except KeyboardInterrupt:
        raise

运行moveit_obstacles_demo.py文件启动避障运动规划

$ rosrun hharm_planning moveit_obstacles_demo.py

运行成功后，稍等片刻
首先出现一个悬浮的桌面，桌面上会出现两个障碍物体
然后机械臂运动到起始位姿

接着避障规划躲过障碍物体，运动到目标位姿
最后运动完成后机械臂回到初始位姿

---

参考：

ROS官方wiki
古月居

---

相关推荐：

ROS笔记（35） 笛卡尔运动规划
ROS笔记（34） 工作空间规划
ROS笔记（33） 关节空间规划
ROS笔记（32） MoveIt!关节控制器
ROS笔记（31） ArbotiX关节控制器

---

谢谢！