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

在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。

使用键盘的简单控制 【机器人|通过外部程序获得Moveit-Rviz的MotionPlanning面板相同的操控能力】有了上面的Topic，我们便可以通过编写程序来操控rviz中的机器人，最主要的就是可以自己编写程序控制机器人上的Interactive Marker，例如使用键盘和手柄控制，而非只是简单地在屏幕上进行非精确地拖动。
在这个例子中，我们使用adsw按键来控制Marker在XY平面的移动，通过上下箭头按键控制X方向上的移动。

#!/usr/bin/env python # coding: UTF-8import rospy import sys, select, termios, ttyfrom moveit_msgs.msg import RobotState from geometry_msgs.msg import PoseStamped from visualization_msgs.msg import InteractiveMarkerUpdate import moveit_commanderclass 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 = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update", InteractiveMarkerUpdate,self.updateGoal) rospy.sleep(2)# 确保rviz能收到消息 # 将IMarker移动到与实际位置一致 self.goalState_pub.publish(self.initState) rospy.sleep(1)# 回调函数通过接收update Topic来更新Marker的目标位置 def updateGoal(self, msg): if len(msg.poses) > 0: self.curPose.pose.position = msg.poses[0].pose.position self.curPose.pose.orientation = msg.poses[0].pose.orientationdef getKey(self): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin],[],[],0) key= sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.ttySettings) return keydef run(self): print("wait key...") while(True) : key = self.getKey() if key=='-': goalPose=self.curPose goalPose.pose.position.z -=0.01 self.marker_pub.publish(goalPose) elif key=='=': goalPose=self.curPose goalPose.pose.position.z +=0.01 self.marker_pub.publish(goalPose) elif key=='a': goalPose=self.curPose goalPose.pose.position.x -=0.01 self.marker_pub.publish(goalPose) elif key == 'd': goalPose =self.curPose goalPose.pose.position.x += 0.01 self.marker_pub.publish(goalPose) elif key == 's': goalPose =self.curPose goalPose.pose.position.y -= 0.01 self.marker_pub.publish(goalPose) elif key == 'w': goalPose =self.curPose goalPose.pose.position.y += 0.01 self.marker_pub.publish(goalPose) if(key == '\x03'):# Ctrl+C breakdef main(): rospy.init_node('marker_test', anonymous=True) app=IMarkerKeyboard() app.run() returnif __name__ == '__main__': main()

使用手柄的简单控制在手柄控制中，使用如下按键或摇杆进行控制：

左摇杆上下： X方向粗控制
左摇杆左右： Y方向粗控制
LEFT-RIGHT: X方向细控制
UP-DOWN: Y方向细控制
右摇杆上下： Z方向粗控制
X-Y： Z方向细控制

这里并没有对旋转进行控制，其实实现也是很简单的。我们可以复用上面的按键或摇杆，设置移动和旋转两种模式，通过一个按钮在两种模式间进行切换即可。

import rospy from moveit_msgs.msg import RobotState from geometry_msgs.msg import PoseStamped from visualization_msgs.msg import InteractiveMarkerUpdate from sensor_msgs.msg importJoy import moveit_commanderclass IMarkerJoystick: 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 = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update", InteractiveMarkerUpdate,self.updateGoal) self.joy_sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1) rospy.sleep(2)# 确保rviz能收到消息 # 将IMarker移动到与实际位置一致 self.goalState_pub.publish(self.initState) rospy.sleep(1)# 回调函数通过接收update Topic来更新Marker的目标位置 def updateGoal(self, msg): if len(msg.poses) > 0: self.curPose.pose.position = msg.poses[0].pose.position self.curPose.pose.orientation = msg.poses[0].pose.orientationdef joyCB(self,msg): goalPose = self.curPose status = XBoxStatus(msg) print(status.select, status.start) print(status.circle, status.center, status.cross, status.square) # 粗调 if status.left_analog_x <=-0.55 : goalPose.pose.position.x -=0.001 elif status.left_analog_x >= 0.55 : goalPose.pose.position.x +=0.001 if status.left_analog_y <= -0.55: goalPose.pose.position.y -= 0.001 elif status.left_analog_y >= 0.55: goalPose.pose.position.y += 0.001 if status.right_analog_y<=-0.55: goalPose.pose.position.z -=0.001 if status.right_analog_y>=0.55: goalPose.pose.position.z += 0.001 # 细调 if status.left: goalPose.pose.position.x -=0.001 if status.right: goalPose.pose.position.x +=0.001 if status.up: goalPose.pose.position.y -=0.001 if status.down: goalPose.pose.position.y +=0.001 if status.square: goalPose.pose.position.z -=0.001 if status.triangle: goalPose.pose.position.z +=0.001 self.marker_pub.publish(goalPose)