ROS-Moveit机械臂追踪二维码(四)

在仿真环境增加相机

<gazebo reference="camera_depth_frame"><sensor name="camera1" type="depth"><always_on>true</always_on><update_rate>20.0</update_rate><camera><horizontal_fov>1.0471975511965976</horizontal_fov><image><format>R8G8B8</format><width>640</width><height>480</height></image><clip><near>0.05</near><far>8.0</far></clip></camera><plugin filename="libgazebo_ros_openni_kinect.so" name="camera_depth_frame_kinect_controller"><baseline>0.1</baseline><alwaysOn>true</alwaysOn><updateRate>10</updateRate><cameraName>camera1</cameraName><imageTopicName>rgb/image_raw</imageTopicName><cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName><depthImageTopicName>depth/image_raw</depthImageTopicName><depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName><pointCloudTopicName>depth/points</pointCloudTopicName><frameName>camera_depth_optical_frame</frameName><pointCloudCutoff>0.3</pointCloudCutoff><distortion_k1>0.0</distortion_k1><distortion_k2>0.0</distortion_k2><distortion_k3>0.0</distortion_k3><distortion_t1>0.0</distortion_t1><distortion_t2>0.0</distortion_t2><!--<CxPrime>0</CxPrime><Cx>0</Cx><Cy>0</Cy><focalLength>0</focalLength><hackBaseline>0</hackBaseline>--></plugin></sensor></gazebo><joint name="camera_rgb_joint" type="fixed"><!-- 以下rpy xyz参数为相机位置可适当修改--><origin rpy="3.142 1.5706 3.142" xyz="0.35 0 1"/><parent link="world"/><child link="camera_rgb_frame"/></joint><link name="camera_rgb_frame"><inertial><mass value="0.001"/><origin xyz="0 0 0"/><inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/></inertial></link><joint name="camera_rgb_optical_joint" type="fixed"><origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/><parent link="camera_rgb_frame"/><child link="camera_rgb_optical_frame"/></joint><link name="camera_rgb_optical_frame"><inertial><mass value="0.001"/><origin xyz="0 0 0"/><inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/></inertial></link><joint name="camera_joint" type="fixed"><origin rpy="0 0 0" xyz="-0.031 0 -0.016"/><parent link="camera_rgb_frame"/><child link="camera_link"/></joint><link name="camera_link"><visual><origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/><geometry><mesh filename="package://robot_arm_urdf/meshes/kinect.dae"/></geometry></visual><collision><origin rpy="0 0 0" xyz="0.0 0.0 0.0"/><geometry><box size="0.07271 0.27794 0.073"/></geometry></collision><inertial><mass value="0.001"/><origin xyz="0 0 0"/><inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/></inertial></link><!-- The fixed joints & links below are usually published by static_transformers launched by the OpenNi launch files. However, for Gazebo simulation we need them, so we add them here.(Hence, don't publish them additionally!) --><joint name="camera_depth_joint" type="fixed"><origin rpy="0 0 0" xyz="0 0 0"/><parent link="camera_rgb_frame"/><child link="camera_depth_frame"/></joint><link name="camera_depth_frame"><inertial><mass value="0.001"/><origin xyz="0 0 0"/><inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/></inertial></link><joint name="camera_depth_optical_joint" type="fixed"><origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/><parent link="camera_depth_frame"/><child link="camera_depth_optical_frame"/></joint><link name="camera_depth_optical_frame"><inertial><mass value="0.001"/><origin xyz="0 0 0"/><inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/></inertial></link><gazebo reference="camera_link"><material>Gazebo/Black</material></gazebo>
</robot>

生成AR码Model

git clone https://github.com/mikaelarguedas/gazebo_models.git

参考其README

移动到/ar_tags/scripts

执行命令格式如下：

$ ./generate_markers_model.py -h
usage: generate gazebo models for AR tagsoptional arguments:-h, --help            show this help message and exit-i IMAGES_DIR, --images-dir IMAGES_DIRdirectory where the marker images are located(default: $HOME/gazebo_models/ar_tags/images)-g GAZEBODIR, --gazebodir GAZEBODIRGazebo models directory (default:$HOME/.gazebo/models)-s SIZE, --size SIZE  marker size in mm (default: 500)-v, --verbose         verbose mode (default: False)-w WHITE_CONTOUR_SIZE_MM, --white-contour-size-mm WHITE_CONTOUR_SIZE_MMAdd white contour around images, default to no contour(default: 0)./generate_markers_model.py -i IMAGE_DIRECTORY -g GAZEBO_MODELS_DIRECTORY -s SIZE_IN_MILLIMETER -w CONTOUR_SIZE_IN_MM

执行以下命令：

./generate_markers_model.py  -s 90

可获得90x90的Ar markers model，模型文件默认保存目录为$HOME/.gazebo/models

建立launch文件

ar_track_param.launch

    <launch><arg name="marker_size" default="9" /> <arg name="max_new_marker_error" default="0.08" /><arg name="max_track_error" default="0.2" /><arg name="cam_image_topic" default="/camera1/rgb/image_raw" /><arg name="cam_info_topic" default="/camera1/rgb/camera_info" /><arg name="output_frame" default="/base_link" /><node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen"><param name="marker_size"           type="double" value="$(arg marker_size)" /><param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" /><param name="max_track_error"       type="double" value="$(arg max_track_error)" /><param name="output_frame"          type="string" value="$(arg output_frame)" /><remap from="camera_image"  to="$(arg cam_image_topic)" /><remap from="camera_info"   to="$(arg cam_info_topic)" /></node></launch>

arg参数可适当修改

建立py文件

moveit_track_demo.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
import tf
import threading
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from ar_track_alvar_msgs.msg import AlvarMarkers, AlvarMarker
from geometry_msgs.msg import PoseStamped, Posex = 0
y = 0
z = 0
ox = 0
oy = 0
oz = 0
zw = 0  
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)               
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('arm_group')
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = moveit_commander.MoveGroupCommander('hand_group')       
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()                       
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)              
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)      
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
gripper.set_goal_joint_tolerance(0.001)        
# 控制机械臂先回到初始化位置
#arm.set_named_target('home')
#arm.go()
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
target_pose = PoseStamped()
a = 1 
def Listener():rospy.init_node('listener', anonymous=True)rospy.Subscriber("/ar_pose_marker",AlvarMarkers,ar_pose, queue_size=1)rospy.spin()
def ar_pose(data):x = data.markers[0].pose.pose.position.xy = data.markers[0].pose.pose.position.yz = data.markers[0].pose.pose.position.zox = data.markers[0].pose.pose.orientation.xoy = data.markers[0].pose.pose.orientation.yoz = data.markers[0].pose.pose.orientation.zow = data.markers[0].pose.pose.orientation.wtarget_pose.header.frame_id = reference_frametarget_pose.header.stamp = rospy.Time.now()     target_pose.pose.position.x =  x-0.08target_pose.pose.position.y = ytarget_pose.pose.position.z = z+0.03target_pose.pose.orientation.x = 0.911822target_pose.pose.orientation.y = -0.0269758target_pose.pose.orientation.z = 0.285694target_pose.pose.orientation.w = -0.293653print(target_pose)# 设置机械臂终端运动的目标位姿arm.set_pose_target(target_pose, end_effector_link)arm.go()# rospy.sleep(1)global aa+=1print(" count ",a) # 关闭并退出moveit#moveit_commander.roscpp_shutdown()#moveit_commander.os._exit(0)print("清除") 
if __name__ == "__main__":Listener()