前期 需要完成机器人操作系统ROS(8)arbotix控制器控制小车运动
物理仿真实验
机器人底盘仿真
我是自己创建了一个工作空间model_gazebo
,创建方法:参考;
如果按照上一篇文章继续操作也可以,记得把mbot_gazebo
换成mbot_description
cd ~/catkin_ws/src/model_gazebo/urdf/xacro
mkdir gazebo
cd gazebo
sudo gedit mbot_base_gazebo.xacro
内容如下:声明xml文件
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
</robot>
编写机器人配置
link添加惯性参数和碰撞属性与gazebo标签
各link的质量(mass)属性声明及其他常量声明
<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="base_mass" value="20" />
<xacro:property name="base_radius" value="0.20"/>
<xacro:property name="base_length" value="0.16"/>
<xacro:property name="wheel_mass" value="2" />
<xacro:property name="wheel_radius" value="0.06"/>
<xacro:property name="wheel_length" value="0.025"/>
<xacro:property name="wheel_joint_y" value="0.19"/>
<xacro:property name="wheel_joint_z" value="0.05"/>
<xacro:property name="caster_mass" value="0.5" />
<xacro:property name="caster_radius" value="0.015"/>
<xacro:property name="caster_joint_x" value="0.18"/>
颜色属性声明
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
<material name="gray">
<color rgba="0.75 0.75 0.75 1"/>
</material>
宏定义 球体惯性矩阵计算
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
宏定义 圆柱体惯性矩阵计算
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
定义驱动轮的宏定义
与参考链接的主要区别在于
link增加惯性属性和碰撞属性
link添加gazebo标签
joint添加传动装置
<xacro:macro name="wheel" params="prefix reflect">
<joint name="${prefix}_wheel_joint" type="continuous">
<origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
<material name="gray" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<gazebo reference="${prefix}_wheel_link">
<material>Gazebo/Gray</material>
</gazebo>
<transmission name="${prefix}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_joint" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_wheel_joint_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
定义前后轮的宏定义
与参考链接的主要区别在于
link增加惯性属性和碰撞属性
link添加gazebo标签
<xacro:macro name="caster" params="prefix reflect">
<joint name="${prefix}_caster_joint" type="continuous">
<origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_caster_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
</geometry>
</collision>
<sphere_inertial_matrix m="${caster_mass}" r="${caster_radius}" />
</link>
<gazebo reference="${prefix}_caster_link">
<material>Gazebo/Black</material>
</gazebo>
</xacro:macro>
定义主体base并添加内容
<xacro:macro name="mbot_base_gazebo">
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<link name="base_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
<wheel prefix="left" reflect="-1"/>
<wheel prefix="right" reflect="1"/>
<caster prefix="front" reflect="-1"/>
<caster prefix="back" reflect="1"/>
</xacro:macro>
添加gazebo控制插件(类似驱动板)
继续在mbot_base_gazebo.xacro
文件中编写
小车需要差速控制器,gazebo里差速控制器的插件是现成的libgazebo_ros_diff_drive.so文件
<gazebo>
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel>Debug</rosDebugLevel>
<publishWheelTF>true</publishWheelTF>
<robotNamespace>/</robotNamespace>
<publishTf>1</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<legacyMode>true</legacyMode>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<wheelSeparation>${wheel_joint_y*2}</wheelSeparation>
<wheelDiameter>${2*wheel_radius}</wheelDiameter>
<broadcastTF>1</broadcastTF>
<wheelTorque>30</wheelTorque>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic>
<odometryFrame>odom</odometryFrame>
<odometryTopic>odom</odometryTopic>
<robotBaseFrame>base_footprint</robotBaseFrame>
</plugin>
</gazebo>
编辑xacro文件
在同级目录下创建sudo gedit mbot_gazebo.xacro
mbot_gazebo.xacro文件内容
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find model_gazebo)/urdf/xacro/gazebo/mbot_base_gazebo.xacro" />
<mbot_base_gazebo/>
</robot>
编辑launch文件
cd ~/catkin_ws/src/mbot_description/launch/xacro
mkdir gazebo
cd gazebo
sudo gedit mbot_base_gazebo.launch
mbot_base_gazebo.launch文件内容
<launch>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find model_gazebo)/urdf/xacro/gazebo/mbot_gazebo.xacro'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
运行
roslaunch model_gazebo mbot_base_gazebo.launch
创建仿真环境
直接添加环境模型
其实就是在上面打开的GAZEBO中进行添加自己想要的仿真工具
模型放置到~/.gazebo/models
文件夹下——------在gazebo的左侧列表点击“insert”(可以看到里面有很多的模型,我们只需要从列表中拖出我们需要的模型放置到仿真环境中就可以)
如果insert
列表为空,或者打开GAZEBRO里面不显示建造的模型,参考最下面错误集合:错误4
最终添加的环境内容
②保存仿真环境
File——Save World As——放置在功能包~/catkin_ws/src/mbot_description/worlds下面(路径自己选择,主要是在 ~/catkin_ws/src/)
最后退出即可。
使用Building Editor创建
1.打开空白的gazebo
roslaunch gazebo_ros empty_world.launch
2.打开Building Editor
3.保存建立的虚拟环境
4.关闭Building Editor
环境效果
5.添加一些model
添加完成的效果
6.保存最终环境
虚拟环境创建完成。
实现gazebo和rviz结合使用仿真
了解就好,下面会具体应用
只需要我们在launch文件中:
设置launch文件的参数处加如.world文件的路径即可这样就可以选择相要使用的仿真环境
<arg name="world_name" value="$(find mbot_gazebo)/worlds/playhouse.world"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
摄像头仿真
需要在完成上面内容后,才可以继续
编辑xacro文件
cd ~/catkin_ws/src/model_gazebo/urdf/xacro
mkdir sensors
cd sensors
sudo gedit camera_gazebo.xacro
内容如下:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
<xacro:macro name="usb_camera" params="prefix:=camera">
<link name="${prefix}_link">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.01" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="${prefix}_link">
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera</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>
</xacro:macro>
</robot>
记得删除中文注释!
编辑带有摄像的xacro文件
在urdf/xacro/gazebo/
目录下创建:sudo gedit mbot_with_camera_gazebo.xacro
文件内容
注:两个需要改的地方:就是第四第五行文件要核对好自己的路径
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find model_gazebo)/urdf/xacro/gazebo/mbot_base_gazebo.xacro" />
<xacro:include filename="$(find model_gazebo)/urdf/xacro/sensors/camera_gazebo.xacro" />
<xacro:property name="camera_offset_x" value="0.17" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.10" />
<joint name="camera_joint" type="fixed">
<origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="camera_link"/>
</joint>
<xacro:usb_camera prefix="camera"/>
<mbot_base_gazebo/>
</robot>
编辑launch文件
带摄像头的机器人launch文件
cd ~/catkin_ws/src/mbot_description/launch/xacro/gazebo
sudo gedit view_mbot_with_camera_gazebo.launch
view_mbot_with_camera_gazebo.launch文件内容
注:两个参数world_name
和robot_description
的路径要核对好
<launch>
<arg name="world_name" value="$(find model_gazebo)/worlds/playhouse.world"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find model_gazebo)/urdf/xacro/sensors/mbot_with_camera_gazebo.xacro'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
运行gazebo
roslaunch mbot_description view_mbot_with_camera_gazebo1.launch
用qt工具查看摄像头当前画面
rqt_image_view
如果提示bash: rqt: 未找到命令 rqt_image_view rqt_graph等等,参考错误5
选择**/camera/image_raw**
启动键盘控制
roslaunch mbot_teleop mbot_teleop.launch
键盘主要有u,i,o,j等。
通过按按键j,u,i,o控制摄像头视角
摄像头仿真就完成了。
Rviz查看摄像头采集的信息
启动rviz `rosrun rviz rviz
添加 add
1.选择机器人模型 RobotModel ,画面出现机器人
2.摄像头信息 image,选择image topic为/camera/image_raw 出现画面信息
通过键盘控制机器人,查看摄像头所得到的效果
错误
如果下面的错误没有你出现的,可以参考这篇 问题解决-----ROS中Gazebo学习的问题解决合集(初次运行黑屏、运行launch文件后闪退、黑屏;gazebo联合rviz出现process has died等的解决办法)
大部分问题都可以参考上面的链接总结,感谢大佬!
错误1
报错原因分析:
1.不接入网络情况下会输出Unable to find uri[model://my1stmodel]
2.接入网络后,如果找不到该模型,会联网在Gazebo远程模型数据库中寻找,由于自己创建的模型名称,在Gazebo模型数据库中是不存在的,所以会一直停在命令行waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, watting...
错误2
gzserver: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreRenderSystem.cpp:546: virtual void Ogre::RenderSystem::setDepthBufferFor(Ogre::RenderTarget*): Assertion bAttached && “A new DepthBuffer for a RenderTarget was created, but after creation” “it says it’s incompatible with that RT”’ failed.
错误原因:
原因是gazebo的版本过低,与Rviz不兼容。ubuntu16.04匹配的ros版本是kinetic,kinetic安装过程中会自动下载低版本的gazebo,然后RViz又是新版本的。
解决办法:
(1)通过下面指令可以看到gazebo版本是7.0.0
xx@ubuntu:~$ gazebo --version
Gazebo multi-robot simulator, version 7.0.0
主要是升级gazebo版本,具体步骤如下:
- 添加下载源
sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
- 添加软件包密钥
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
- 更新系统下载源
sudo apt-get update
- 升级gazebo
安装gazebo7或以上的版本
sudo apt-get install gazebo8 或gazebo9
- 测试gazebo版本
xx@ubuntu:~$ gazebo --version
Gazebo multi-robot simulator, version 7.16.1
- gazebo升级后,gazebo可正常运行,而且可以加载模型
如果还是不行,可能是显卡驱动比较陈旧,需要更新,更新英伟达的驱动
错误3
[gazebo_gui-3] process has died [pid 2238, exit code 134…(略)
方法一:gazebo启用3D加速选项方面存在一些问题,可从VM设置中禁用设置3D加速选项。禁用该选项后,仿真环境运行会比较缓慢,但可以正常工作。如图所示:
方法二:更改SVGA_VGPU10变量:
1先在终端执行export SVGA_VGPU10=0
2然后再执行roslaunch语句,比如我的是roslaunch ur_gazebo ur3.launch
错误4
Error [parser.cc:581] Unable to find uri[model://sun] Error [parser.cc:581] Unable to find uri[model://ground_plane]
参考https://aichen.blog.csdn.net/article/details/124391936
解决: 第一次下载安装gazebo
的时候没有将模型文件models下载下来,所以缺少了sun
和ground plane
等模型。我们按照他说的链接,或者到github中下载下来放到.gazebo
文件夹中即可。所需要的models文件我已经放到了百度云盘中,可以直接下载。
注意,要在.gazebo中建立models文件夹,把下载好的众多包放到里面,不要建立model,我第一次少写了一个s,结果加载不到。
如果在/home/use_name
下找不到`.hazebo’文件,原因是它被隐藏了,在当前页面状态中,按下ctrl+h显示隐藏文件,如
错误5
bash: rqt: 未找到命令 rqt_image_view rqt_graph等等
解决办法:
可能是rqt被不小心卸载了,重装就好
sudo apt-get install ros-kinetic-rqt
sudo apt-get install ros-kinetic-rqt-graph
sudo apt-get install ros-kinetic-rqt-common-plugins
参考
教程参考:
ROS——ArbotiX+Rviz仿真
✳✳ROS——Gazebo物理仿真环境搭建
ros + gazebo未报错却加载不出来机器人模型 解决办(这里面的代码不添加注释可以直接复制)
[ROS——Gazebo物理仿真安装
解决的问题参考链接
ros + gazebo未报错却加载不出来机器人模型 解决办
Assertion `bAttached && “A new DepthBuffer for a RenderTarget was created, but after creation“ “it s
解决 gazebo_gui-3 process has died pid 2238, exit code 134
https://blog.csdn.net/Will_Ye/article/details/116428880
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)