目录
本文承接此文：一、适用于最新moveit版本的mycobot 350 pi 机械臂 gazebo+moveit+rviz联合仿真
一、github下载gazebo仿真用D435i模型与gazebo传感器插件文件
官网链接：https://github.com/nilseuropa/realsense_ros_gazebo
其实看到github下面的readme已经有相机插入教程了：
一会我们确实就是按这个模版，然后调整一下位置，加入。
二、单独测试D435i
- 将整个gazebo_camera_sim功能包复制到catkin_ws/src。
- catkin_make编译一下
- 执行以下指令测试相机：
roslaunch realsense_ros_gazebo simulation.launch
- 另开 一个终端输入以下指令检查话题是否正常发布：
rostopic list
执行结果如下，左边为相机gazebo仿真，右边为话题清单，可以看到右边已经有我们需要的rgb话题/d435/color/image_raw，以及其他的深度信息话题/d435/depth/image_raw：
以上步骤顺利完成后再进行下一步，如果遇到错误欢迎在评论区发出，或者直接私信我，我会尽我所能一一解答。
三、提取模型文件
-
先在我们的mycobot_description功能包中找到我们放机械臂模型的urdf文件夹，然后在里面新建个camera文件夹（没什么特别用处，只是为了美观）：
-
解压我们刚下载的realsense_ros_gazebo-master，打开realsense_ros_gazebo-master/xacro，将里面三个文件都拷贝到上述的camera文件夹里面，这三个文件包含了很多realsense相机的模型与gazebo传感器配置插件，只需要在原先的xacro模型里加入包含代码就可以直接使用相机的全部传感器功能。
-
打开realsense_ros_gazebo-master/meshes/，将里面的realsense_d435.stl文件同样移动到camera文件夹里：
-
打开depthcam文件，看到的第一个相机就是我们要的D435i：
-
然后往下拉到132行，看到：
<horizontal_fov>${69.4*deg_to_rad}</horizontal_fov>
把69.4改成30：
<horizontal_fov>${30*deg_to_rad}</horizontal_fov>
为什么要这么改呢，这个69.4其实就是视野角度，玩过fps的都知道，我们在调画质的时候可以调视野广度，如果你想看到更多的敌人，就需要把视野广度拉到最大，这个69.4其实就是相机能看到69.4度的画面。我不清楚真实的D435i相机是否能达到这么大的视野广度，但为了匹配gazebo中显示的相机成像线，我们把这个广度改成30会提高仿真的真实度。当然大家也可以不改，不影响视觉识别，可以试试看不改会怎样。
- 现在看到第4行：
<xacro:include filename="$(find realsense_ros_gazebo)/xacro/inertia_calc.xacro"/>
- 改成：
<xacro:include filename="$(find mycobot_description)/urdf/mycobot_320_pi_2022/camera/inertia_calc.xacro"/>
这个很好理解，就是包含的inertia_calc.xacro文件路径要改成我们自己的。
- 同样的，看到第40行：
<mesh filename="package://realsense_ros_gazebo/meshes/realsense_d435.stl" />
- 改成：
<mesh filename="package://mycobot_description/urdf/mycobot_320_pi_2022/camera/realsense_d435.stl" />
这个也很好理解，导入我们刚刚移动到camera文件夹里的stl文件。
- 现在看到248行，看到下面定义的是另一个realsense_R200相机，这里我们用不到，直接将整个xacro标签删去，也就是从248行删到448行。
- 修改好的xacro文件：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mycobot_description)/urdf/mycobot_320_pi_2022/camera/inertia_calc.xacro"/>
<!-- INTEL REALSENSE D435 -->
<xacro:macro name="realsense_d435" params="sensor_name parent_link *origin rate">
<xacro:property name="d435_cam_depth_to_left_ir_offset" value="0.0"/>
<xacro:property name="d435_cam_depth_to_right_ir_offset" value="-0.050"/>
<xacro:property name="d435_cam_depth_to_color_offset" value="0.015"/>
<xacro:property name="d435_cam_width" value="0.090"/>
<xacro:property name="d435_cam_height" value="0.025"/>
<xacro:property name="d435_cam_depth" value="0.02505"/>
<xacro:property name="d435_cam_mass" value="0.564"/>
<xacro:property name="d435_cam_mount_from_center_offset" value="0.0149"/>
<xacro:property name="d435_cam_depth_px" value="${d435_cam_mount_from_center_offset}"/>
<xacro:property name="d435_cam_depth_py" value="0.0175"/>
<xacro:property name="d435_cam_depth_pz" value="${d435_cam_height/2}"/>
<!-- camera body, with origin at bottom screw mount -->
<joint name="${sensor_name}_joint" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent_link}"/>
<child link="${sensor_name}_bottom_screw_frame" />
</joint>
<link name="${sensor_name}_bottom_screw_frame"/>
<joint name="${sensor_name}_link_joint" type="fixed">
<origin xyz="0 ${d435_cam_depth_py} ${d435_cam_depth_pz}" rpy="0 0 0"/>
<parent link="${sensor_name}_bottom_screw_frame"/>
<child link="${sensor_name}_link" />
</joint>
<link name="${sensor_name}_link">
<visual>
<origin xyz="${d435_cam_mount_from_center_offset} ${-d435_cam_depth_py} 0" rpy="${pi/2} 0 ${pi/2}"/>
<geometry>
<mesh filename="package://mycobot_description/urdf/mycobot_320_pi_2022/camera/realsense_d435.stl" />
</geometry>
</visual>
<collision>
<origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
<geometry>
<box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
</geometry>
</collision>
<inertial>
<mass value="${d435_cam_mass}" />
<origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
<xacro:box_inertia m="${d435_cam_mass}" x="${d435_cam_depth}" y="${d435_cam_width}" z="${d435_cam_height}"/>
</inertial>
</link>
<!-- camera depth joints and links -->
<joint name="${sensor_name}_depth_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="${sensor_name}_link"/>
<child link="${sensor_name}_depth_frame" />
</joint>
<link name="${sensor_name}_depth_frame"/>
<joint name="${sensor_name}_depth_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
<parent link="${sensor_name}_depth_frame" />
<child link="${sensor_name}_depth_optical_frame" />
</joint>
<link name="${sensor_name}_depth_optical_frame"/>
<!-- camera left IR joints and links -->
<joint name="${sensor_name}_left_ir_joint" type="fixed">
<origin xyz="0 ${d435_cam_depth_to_left_ir_offset} 0" rpy="0 0 0" />
<parent link="${sensor_name}_depth_frame" />
<child link="${sensor_name}_left_ir_frame" />
</joint>
<link name="${sensor_name}_left_ir_frame"/>
<joint name="${sensor_name}_left_ir_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
<parent link="${sensor_name}_left_ir_frame" />
<child link="${sensor_name}_left_ir_optical_frame" />
</joint>
<link name="${sensor_name}_left_ir_optical_frame"/>
<!-- camera right IR joints and links -->
<joint name="${sensor_name}_right_ir_joint" type="fixed">
<origin xyz="0 ${d435_cam_depth_to_right_ir_offset} 0" rpy="0 0 0" />
<parent link="${sensor_name}_depth_frame" />
<child link="${sensor_name}_right_ir_frame" />
</joint>
<link name="${sensor_name}_right_ir_frame"/>
<joint name="${sensor_name}_right_ir_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
<parent link="${sensor_name}_right_ir_frame" />
<child link="${sensor_name}_right_ir_optical_frame" />
</joint>
<link name="${sensor_name}_right_ir_optical_frame"/>
<!-- camera color joints and links -->
<joint name="${sensor_name}_color_joint" type="fixed">
<origin xyz="0 ${d435_cam_depth_to_color_offset} 0" rpy="0 0 0" />
<parent link="${sensor_name}_depth_frame" />
<child link="${sensor_name}_color_frame" />
</joint>
<link name="${sensor_name}_color_frame"/>
<joint name="${sensor_name}_color_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
<parent link="${sensor_name}_color_frame" />
<child link="${sensor_name}_color_optical_frame" />
</joint>
<link name="${sensor_name}_color_optical_frame"/>
<!-- gazebo plugin -->
<xacro:property name="deg_to_rad" value="0.01745329251994329577" />
<gazebo reference="${sensor_name}_link">
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
<gravity>1</gravity>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<kp>1e+13</kp>
<kd>1</kd>
<sensor name="${sensor_name}_color" type="camera">
<camera name="${sensor_name}">
<horizontal_fov>${30*deg_to_rad}</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>RGB_INT8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>1</visualize>
</sensor>
<sensor name="${sensor_name}_ired1" type="camera">
<camera name="${sensor_name}">
<horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>L_INT8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.05</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>90</update_rate>
<visualize>0</visualize>
</sensor>
<sensor name="${sensor_name}_ired2" type="camera">
<camera name="${sensor_name}">
<horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>L_INT8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.05</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>90</update_rate>
<visualize>0</visualize>
</sensor>
<sensor name="${sensor_name}_depth" type="depth">
<camera name="${sensor_name}">
<horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.100</stddev>
</noise>
</camera>
<always_on>1</always_on>
<update_rate>90</update_rate>
<visualize>0</visualize>
</sensor>
</gazebo>
<gazebo>
<plugin name="${sensor_name}" filename="librealsense_gazebo_plugin.so">
<prefix>${sensor_name}_</prefix>
<depthUpdateRate>${rate}</depthUpdateRate>
<colorUpdateRate>${rate}</colorUpdateRate>
<infraredUpdateRate>${rate}</infraredUpdateRate>
<depthTopicName>depth/image_raw</depthTopicName>
<depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
<colorTopicName>color/image_raw</colorTopicName>
<colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
<infrared1TopicName>infra1/image_raw</infrared1TopicName>
<infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
<infrared2TopicName>infra2/image_raw</infrared2TopicName>
<infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
<colorOpticalframeName>${sensor_name}_color_optical_frame</colorOpticalframeName>
<depthOpticalframeName>${sensor_name}_depth_optical_frame</depthOpticalframeName>
<infrared1OpticalframeName>${sensor_name}_infrared1_optical_frame</infrared1OpticalframeName>
<infrared2OpticalframeName>${sensor_name}_infrared2_optical_frame</infrared2OpticalframeName>
<rangeMinDepth>0.2</rangeMinDepth>
<rangeMaxDepth>10.0</rangeMaxDepth>
<pointCloud>false</pointCloud>
<pointCloudTopicName>depth/color/points</pointCloudTopicName>
<pointCloudCutoff>0.25</pointCloudCutoff>
<pointCloudCutoffMax>9.0</pointCloudCutoffMax>
</plugin>
</gazebo>
</xacro:macro>
</robot>
大家根据自的路径自行修改！
四、在机械臂xacro文件中加入相机
- 打开我们机械臂的xacro模型文件new_mycobot_pro_320_pi_moveit_2022.urdf.xacro，看到145行，在后面加入这段代码：
<xacro:include filename="$(find mycobot_description)/urdf/mycobot_320_pi_2022/camera/depthcam.xacro"/>
<xacro:realsense_d435 sensor_name="d435" parent_link="link6" rate="10">
<origin rpy="0 -1.5708 1.5708 " xyz="0 -0.05 -0.015"/>
</xacro:realsense_d435>
这段就是包含D435i模型文件，然后设置他的parent_link为link6，然后配置了加入的位置。位置如果觉得不合适的话可以自己调整一下。
- 然后我们运行启动指令测试相机放置情况与话题发布情况：
roslaunch mycobot_moveit_config demo_gazebo.launch
- 运行结果：
- 输入指令看话题发布情况：
也看到了我们需要的/d435/color/image_raw，相机配置成功。
五、python订阅rgb话题并做视觉识别
至此gazebo仿真环境我们顺利搭建完毕，接下来是是对发布的仿真信息进行处理，我们接下来要做的是编写python脚本读取/d435/color/image_raw话题信息，作为虚拟摄像头展示出来，然后结合opencv视觉识别算法计算红色块中心点坐标。
- ubuntu自带了python，所以我们不需要自己安装，这里需要安装个python-is-python3依赖来方便我们使用pyhton指令代替python3指令，以及一些opencv的依赖：
sudo apt-get install python-is-python3
sudo apt-get install ros-noetic-cv-bridge
sudo apt-get install python3-opencv
- 在catkin/src路径下输入以下指令以创建python功能包：
catkin_create_pkg mycobot_moveit_py_image_sub cv_bridge rospy sensor_msgs
包含三个依赖cv_bridge rospy sensor_msgs，分别是opencv库 python编译库 gazebo传感器信息类型库。
- 输入以下指令新建文件夹script，装python文件：
mkdir script
- 进入script文件夹后新建python文件，名为mycobot_moveit_py_image_sub.py：
touch mycobot_moveit_py_image_sub.py
- copy源码：
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped
from cv_bridge import CvBridge, CvBridgeError
import cv2
import numpy as np
from collections import deque
# 设定红色阈值，HSV空间
redLower = np.array([0, 80, 50])
redUpper = np.array([8, 255, 220])
mybuffer = 64 # 初始化追踪点的列表
pts = deque(maxlen=mybuffer)
# 创建一个发布者
pub = rospy.Publisher('/red_object_center', PointStamped, queue_size=10)
def image_callback(msg):
bridge = CvBridge()
try:
# 将ROS图像消息转换为OpenCV图像
cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
return
# 获取图像的尺寸
height, width, _ = cv_image.shape
center_x = width // 2
center_y = height // 2
# 转到HSV空间
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
# 根据阈值构建掩膜
mask = cv2.inRange(hsv, redLower, redUpper)
# 腐蚀操作
mask = cv2.erode(mask, None, iterations=2)
# 膨胀操作，去除噪点
mask = cv2.dilate(mask, None, iterations=2)
# 轮廓检测
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None # 初始化瓶盖圆形轮廓质心
# 如果存在轮廓
if len(cnts) > 0:
# 找到面积最大的轮廓
c = max(cnts, key=cv2.contourArea)
# 确定面积最大的轮廓的外接圆
((x, y), radius) = cv2.minEnclosingCircle(c)
# 计算轮廓的矩
M = cv2.moments(c)
# 计算质心
center = (int(M['m10'] / M['m00']), int(M['m01'] / M['m00']))
cv2.circle(cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(cv_image, center, 5, (0, 0, 255), -1)
# 发布坐标信息
point_msg = PointStamped()
point_msg.header.stamp = rospy.Time.now()
point_msg.header.frame_id = "camera_frame" # 假设相机帧名为camera_frame
point_msg.point.x = (-center[0] + 319)*0.0002#*0.0003669
point_msg.point.y = 0
point_msg.point.z = (-center[1] + 239)*0.0002#*0.0003669
pub.publish(point_msg)
# 显示图像
cv2.imshow("RGB Image", cv_image)
cv2.waitKey(30)
def main():
rospy.init_node('rgb_image_subscriber', anonymous=True)
# 订阅RGB图像话题
rospy.Subscriber("/d435/color/image_raw", Image, image_callback)
# 保持节点运行
rospy.spin()
if __name__ == '__main__':
main()
简单解释一下源码的关键部分：
这句代码表示订阅/d435/color/image_raw话题，然后传入image信息，执行image_callback回调函数：
rospy.Subscriber("/d435/color/image_raw", Image, image_callback)
回调函数主要工作是转换图像信息，进行视觉识别算法，图像显示，发布识别结果到话题/red_object_center。
创建订阅者，然后发布计算结果：
pub = rospy.Publisher('/red_object_center', PointStamped, queue_size=10)
...
pub.publish(point_msg)
这段源码有兴趣的朋友可以直接copy到chatgpt或者其他ai平台，让它辅助解析，这样学起来非常快，我在此不过多赘述。
推荐一个很好用的国内大模型：https://www.deepseek.com/
- 源码写完保存退出，回到catkin工作空间编译：
catkin_make
- 编译通过后可以使用了，我们现在来启动下程序：
roslaunch mycobot_moveit_config demo_gazebo.launch
- 再开一个终端：
rosrun mycobot_moveit_py_image_sub mycobot_moveit_py_image_sub.py
我们可以把系统自带的任何红色物体放置在镜头前方：
- 输入指令查看/red_object_center话题上是否有我们想要的结果：
rostopic echo /red_object_center
成功显示目标点与中心点偏移的距离：
这边为什么是x轴和z轴呢？因为在我们机械臂的末端坐标系里，上下是z，左右是x，前后是y，这段源码是已经做了坐标轴转换和坐标值转换的，其实就是将原本的y换成了z，将原本在左下角的原点坐标坐标换到了正中间，将坐标数值乘了0.0002来转换成gazebo中的坐标值。
至此，相机加入模型，读取相机图像数据并视觉识别部分已完成！大家如果有问题欢迎评论区提出，我会尽力一一回答！
4564
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
