在Moveit-Rviz启动之后,在机器人的末端会显示一个Interactive Marker用来指示目标方位,可以通过鼠标来移动和旋转该Marker,操作时会有一个虚拟的机器人随之运动。当按下MotionPlanning面板中的Plan按钮便会显示从当前方位到目标方位的一个运动轨迹。当按下Execute按钮后机器人就会实际运动到目标方位。
当勾选MotionPlanning面板中的"Allow External Comm."选项的时候,在ROS上就会出现如下的几个Topic:
rostopic pub /rviz/moveit/select_planning_group std_msgs/String "hand"
另外,Moveit发布如下的Topic
有了上面的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 = 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.orientation
def 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 key
def 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
break
def main():
rospy.init_node('marker_test', anonymous=True)
app=IMarkerKeyboard()
app.run()
return
if __name__ == '__main__':
main()
在手柄控制中,使用如下按键或摇杆进行控制:
这里并没有对旋转进行控制,其实实现也是很简单的。我们可以复用上面的按键或摇杆,设置移动和旋转两种模式,通过一个按钮在两种模式间进行切换即可。
import rospy
from moveit_msgs.msg import RobotState
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import InteractiveMarkerUpdate
from sensor_msgs.msg import Joy
import moveit_commander
class 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.orientation
def 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)