通过外部程序获得Moveit-Rviz的MotionPlanning面板相同的操控能力

最新推荐文章于 2026-07-27 08:15:51 发布

原创

最新推荐文章于 2026-07-27 08:15:51 发布 · 1.8k 阅读

在Moveit-Rviz启动之后，在机器人的末端会显示一个Interactive Marker用来指示目标方位，可以通过鼠标来移动和旋转该Marker，操作时会有一个虚拟的机器人随之运动。当按下MotionPlanning面板中的Plan按钮便会显示从当前方位到目标方位的一个运动轨迹。当按下Execute按钮后机器人就会实际运动到目标方位。

Moveit在rviz中提供的操控Topic

当勾选MotionPlanning面板中的"Allow External Comm."选项的时候，在ROS上就会出现如下的几个Topic:

/rviz/moveit/plan : 和Rviz中MotionPlanning面板提供的Plan按钮功能一样，该topic的消息类型为std_msgs/Empty，发送一个这样的消息到该Topic就相当与按下了Plan按钮。
/rviz/moveit/execute : 和Rviz中MotionPlanning面板提供的Execute按钮功能一样
/rviz/moveit/select_planning_group : 用于选择你想要控制的Planning Group的Topic。其消息类型为std_msgs/String。你可以在命令终端下选择Planning Group，例如
```
rostopic pub /rviz/moveit/select_planning_group std_msgs/String "hand"
```
/rviz/moveit/update_{goal,start}_state : 和MotionPlanning面板中"State State"和"Goal State"下选框中的"current"是同样的功能。
/rviz/moveit/update_custom_{goal,start}_state : 该Topic用于给机器人设置一个指定的状态，其消息类型为moveit_msgs/RobotState。
/rviz/moveit/move_marker/goal_#{link_name} : 指定交互式marker的方位的Topic。link_name是Planning Group中的末端连杆的名称，例如"/rviz/moveit/move_marker/goal_panda_link8。其中"panda_link8"为panda机器人本体的最后一个连杆。该Topic的消息类型为geometry_msgs/PoseStamped。

另外，Moveit发布如下的Topic

/rviz_moveit_motion_planning_display/robot_interactive_interactive_marker_topic/update : 该Topic会发布经过确认的有效的IMarker方位。我们可以用来更新IMarker的实际goalPose。
/execute_trajectory/status : 当moveit通过Execute执行实际的轨迹运动时，就会通过该Topic发布执行的状态。因此当我们进行实际的轨迹运动时就可以通过该Topic来检查完成结果。该Topic类型为actionlib_msgs/GoalStatusArray。

使用键盘的简单控制

有了上面的Topic，我们便可以通过编写程序来操控rviz中的机器人，最主要的就是可以自己编写程序控制机器人上的Interactive Marker，例如使用键盘和手柄控制，而非只是简单地在屏幕上进行非精确地拖动。

在这个例子中，我们使用adsw按键来控制Marker在XY平面的移动，通过上下箭头按键控制X方向上的移动。

#!/usr/bin/env python
# coding: UTF-8

import rospy
import sys, select, termios, tty

from moveit_msgs.msg import RobotState
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import InteractiveMarkerUpdate
import moveit_commander

class IMarkerKeyboard:
    def __init__(self):
        self.pre_pose= PoseStamped()
        self.planning_groups_tips = {
   
   }
        self.ttySettings=termios.tcgetattr(sys.stdin)
        # 获得机器人的当前实际位置
        group_name = "arm"  # 这是对机器人进行moveit配置定义的关节组名称
        self.group = moveit_commander.MoveGroupCommander(group_name)
        self.robot = moveit_commander.RobotCommander()
        self.curPose=self.group.get_current_pose()
        self.initState= self.robot.get_current_state()
        # 创建相关
        self.goalState_pub = rospy.Publisher("/rviz/moveit/update_custom_goal_state",
                                           RobotState,queue_size=5)
        self.marker_pub = rospy.Publisher("/rviz/moveit/move_marker/goal_link_6",
                                           PoseStamped,queue_size=5)
        self.update_sub