ardupilot_gazebo仿真(二)
标签(空格分隔): 未分类
在模型中添加sensor
gezebo官网-sensor部分教程
gezebo官网-基础部分教程
Gazebo plugins in ROS
以camera为例
<sensor name="camera" type="camera">
<pose>0 0 0 0 -1.57 0</pose>
<camera>
<horizontal_fov>2.0</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>15000</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>1</visualize>
</sensor>
其中pose是3个position\((x,y,z)\)和1个旋转矢量\((\theta,\phi,\psi)\)
在ardupilot_gazebo中ardupilot_gazebo/models_gazebo/gimbal_small_2d是云台的模型,其中包括了相机的模型。通过更改pose可以改变相机的朝向和位置。
如果不对这个目录下的model.sdf进行修改,我们只能得到在gazebo中的图像,并不能与ROS进行通信,也就是说不能在rostopic中看到与camera相关的topic
所以需要添加 senosrplugin
<robot>
... robot description ...
<link name="sensor_link">
... link description ...
</link>
<gazebo reference="sensor_link">
<sensor type="camera" name="camera1">
... sensor parameters ...
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
... plugin parameters ..
</plugin>
</sensor>
</gazebo>
</robot>
以camera为例
<!-- camera -->
<gazebo reference="camera_link">
<sensor type="camera" name="camera1">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>rrbot/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
相关注释在官网介绍中可以看到。
下面是我们针对ardupilot_gazebo中的gimbal_small_2d模型进行的修改。在ardupilot_gazebo/models_gazebo/gimbal_small_2d目录下找到model.sdf,其中与 camera有关的代码如下:
<sensor name="camera" type="camera">
<pose>0 0 0 0 -1.57 0</pose>
<camera>
<horizontal_fov>2.0</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>15000</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>1</visualize>
</sensor>
在其中加入插件
<sensor name="camera" type="camera">
<pose>0 0 0 0 -1.57 0</pose>
<camera>
<horizontal_fov>2.0</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>15000</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<always_on>1</always_on>
<update_rate>30</update_rate>
<cameraName>rrbot/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<visualize>1</visualize>
</plugin>
</sensor>
运行代码可以看到在rostopic中有了camera的相关topic
运行rviz然后添加imgae可以看到图像
rosrun rviz rviz
add image
/rrbot/camera1/image_raw