在自己的xacro模型的基础上加入常见传感器在rviz中显示+ArbotiX+rviz仿真
·
本篇文章需要在下面这篇的基础上实现的，当然如果你已经有了自己的xacro模型并能在rviz中显示的话可以跳过O(∩_∩)O哈哈~
一、机器人加上摄像头
在自己的工作文件夹下的urdf中新建camera.xacro
touch camera.xacro
添加代码camera.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>
</xacro:macro>
</robot>如下图所示把代码粘进去并保存好哦！
好啦！我们现在就可以进行进行像叠罗汉一样的操作把我们的机器人的xacro和摄像头的camera.xacro进行一个合并啦！
新建mrobot_with_camera.urdf.xacro文件
我们新建一个mrobot_with_camera.urdf.xacro文件用来读取robot1_base.xacro（机器人模型文件）和camera.xacro（摄像头模型文件）
mrobot_with_camera.urdf.xacro代码如下：
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find myrobot)/urdf/robot1_base.xacro" />
<xacro:include filename="$(find myrobot)/urdf/camera.xacro" />
<xacro:property name="camera_offset_x" value="0.1" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.2" />
<xacro:usb_camera prefix="camera"/>
<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>
</robot>
xacro注意事项：
1. 记得修改读取的xacro文件导向的确实是你自己的模型文件位置
- 这个myrobot改成你自己功能包的名字以及保证你的功能包的地址设置是正确的
- 确定拼接后的xacro文件确实是导向为正确位置
<xacro:include filename="$(find myrobot)/urdf/robot1_base.xacro" />
<xacro:include filename="$(find myrobot)/urdf/camera.xacro" />2. 这个地方的x,y,z是摄像头和基准的位置，这个地方需要你进入模型后进行调整使得相机以合适的位置放到机器人上面
<xacro:property name="camera_offset_x" value="0.1" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.02" />3.这个部分就是机器和摄像头连接的核心部分，大家可以对应观察一下camera.xacro文件的内容
<xacro:usb_camera prefix="camera"/>
<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>在launch文件夹下面新建xiangji.launch
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<param name="robot_description" command="$(find xacro)/xacro $(find myrobot)/urdf/mrobot_with_camera.urdf.xacro" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 添加 Arbotix 节点 -->
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find myrobot)/config/fake_mrobot_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
</launch>
launch注意：
1. 这个launch文件我是带了Arbotix 节点，关于Arbotix 节点仿真控制小车运动在文章后面我会说明，这里先不讨论
a.你可以选择把这段代码给删除了
<!-- 添加 Arbotix 节点 -->
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find myrobot)/config/fake_mrobot_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>b.或者新建终端安装对应包
echo $ROS_DISTRO查看自己的ros版本并进行安装对应版本的依赖包，我的ros版本是melodic所以进行修改为melodic
sudo apt-get install ros-melodic-arbotix-*下面我会给大家看一下其它ros版本的图片介绍这个过程
c.在功能包的目录下新建一个config文件夹，并新建一下fake_mrobot_arbotix.yaml
mkdir configcd config
touch fake_mrobot_arbotix.yaml
fake_mrobot_arbotix.yaml内容为：
controllers: {
base_controller: {
type: diff_controller,
base_frame_id: base_footprint,
base_width: 0.26,
ticks_meter: 4100,
Kp: 12,
Kd: 12,
Ki: 0,
Ko: 50,
accel_limit: 1.0
}
}2.确保xacro模型的位置路径是正确的
<param name="robot_description" command="$(find xacro)/xacro $(find myrobot)/urdf/mrobot_with_camera.urdf.xacro" />运行launch文件
好！现在我们一切准备就绪！开始运行launch文件啦！
roslaunch myrobot xiangji.launch
添加RobotModel
关于摄像头与机器人的位置调整的方法如下：
<xacro:property name="camera_offset_x" value="0.1" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.02" />
及x=0.1，y=0，z=0.02时：
可以看到摄像头是嵌入在小车里面的这就说明了位置并不是很OK的嘞！
那么我们调整一下
重新设置一下
<xacro:property name="camera_offset_x" value="0.1" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.2" />
及x=0.1，y=0，z=0.2时：
现在看起来就舒服多了是不是！
二、机器人加上kinect摄像头
在自己的工作文件夹下的urdf中新建kinect.xacro
touch kinect.xacro
添加代码kinect.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">
<xacro:property name="M_PI" value="3.14159265358979323846"/>
<xacro:macro name="kinect_camera" params="prefix:=kinect">
<link name="${prefix}_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
<geometry>
<mesh filename="package://myrobot/meshes/kinect.dae" />
</geometry>
</visual>
<collision>
<geometry>
<box size="0.07 0.3 0.09"/>
</geometry>
</collision>
</link>
<joint name="${prefix}_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
<parent link="${prefix}_link"/>
<child link="${prefix}_frame_optical"/>
</joint>
<link name="${prefix}_frame_optical"/>
</xacro:macro>
</robot>
注意下面这个代码，需要下载安装一下kinect摄像头的贴图
kinect摄像头贴图meshes.zip
链接: https://pan.baidu.com/s/1cbKFQdQFJI5oKFtfYIqIEQ?pwd=gki2 提取码: gki2
<mesh filename="package://myrobot/meshes/kinect.dae" />复制到功能包的根目录然后解压按照这个路径或者修改哦！
如下图所示把代码粘进去并保存好哦！
新建mrobot_with_kinect.urdf.xacro
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 加载基础机器人模型和Kinect模型 -->
<xacro:include filename="$(find myrobot)/urdf/robot1_base.xacro" />
<xacro:include filename="$(find myrobot)/urdf/kinect.xacro" />
<!-- 定义Kinect的偏移 -->
<xacro:property name="kinect_offset_x" value="-0.06" />
<xacro:property name="kinect_offset_y" value="0" />
<xacro:property name="kinect_offset_z" value="0.15" />
<!-- 引入基础机器人主体 -->
<mrobot_body/>
<!-- 调用Kinect相机宏 -->
<xacro:kinect_camera prefix="kinect"/>
<!-- Kinect相机的连接关节 -->
<joint name="kinect_frame_joint" type="fixed">
<origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="0 0 0" />
<parent link="base_link"/> <!-- 修改 parent link 为你实际基于的链接 -->
<child link="kinect_link"/>
</joint>
</robot>
在launch文件夹下新建deepxiangji.launch文件
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<param name="robot_description" command="$(find xacro)/xacro $(find myrobot)/urdf/mrobot_with_kinect.urdf.xacro" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 添加 Arbotix 节点 -->
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find myrobot)/config/fake_mrobot_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
</launch>
运行launch
详细配置环境请查看上面camer.xacro文件的launch配置方法
roslaunch myrobot deepxiangji.launch
三、机器人加上雷达
在自己的工作文件夹下的urdf中新建rplidar.xacro
touch rplidar.xacro
添加代码rplidar.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser">
<xacro:macro name="rplidar" params="prefix:=laser">
<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>
<cylinder length="0.05" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<cylinder length="0.06" radius="0.05"/>
</geometry>
</collision>
</link>
</xacro:macro>
</robot>如下图所示把代码粘进去并保存好哦！
新建mrobot_with_rplidar.urdf.xacro
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 加载基础机器人模型和激光雷达模型 -->
<xacro:include filename="$(find myrobot)/urdf/robot1_base.xacro" />
<xacro:include filename="$(find myrobot)/urdf/rplidar.xacro" />
<!-- 定义激光雷达的偏移 -->
<xacro:property name="rplidar_offset_x" value="0" />
<xacro:property name="rplidar_offset_y" value="0" />
<xacro:property name="rplidar_offset_z" value="0.18" />
<!-- 引入基础机器人主体 -->
<mrobot_body/>
<!-- 调用激光雷达宏以定义链接 -->
<xacro:rplidar prefix="laser"/>
<!-- 定义激光雷达的连接关节 -->
<joint name="rplidar_joint" type="fixed">
<origin xyz="${rplidar_offset_x} ${rplidar_offset_y} ${rplidar_offset_z}" rpy="0 0 0" />
<parent link="base_link"/> <!-- 这里根据实际的父链接调整 -->
<child link="laser_link"/> <!-- 使用激光雷达的链接名称 -->
</joint>
</robot>
在launch文件夹下面新建leida.launch文件
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<param name="robot_description" command="$(find xacro)/xacro $(find myrobot)/urdf/mrobot_with_rplidar.urdf.xacro" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 添加 Arbotix 节点 -->
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find myrobot)/config/fake_mrobot_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
</launch>
运行leida.launch文件
详细配置环境请查看上面camer.xacro文件的launch配置方法
roslaunch myrobot leida.launch
四、机器人三合一
现在我们把三个传感器串在一起吧
新建zong.xacro
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find myrobot)/urdf/robot1_base.xacro" />
<xacro:include filename="$(find myrobot)/urdf/kinect.xacro" />
<xacro:include filename="$(find myrobot)/urdf/rplidar.xacro" />
<xacro:include filename="$(find myrobot)/urdf/camera.xacro" />
<xacro:property name="kinect_offset_x" value="-0.06" />
<xacro:property name="kinect_offset_y" value="0" />
<xacro:property name="kinect_offset_z" value="0.15" />
<xacro:property name="rplidar_offset_x" value="0" />
<xacro:property name="rplidar_offset_y" value="0" />
<xacro:property name="rplidar_offset_z" value="0.18" />
<xacro:property name="camera_offset_x" value="0.1" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.15" />
<mrobot_body/>
<xacro:kinect_camera prefix="kinect"/>
<joint name="kinect_frame_joint" type="fixed">
<origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="kinect_link"/>
</joint>
<xacro:rplidar prefix="laser"/>
<joint name="rplidar_joint" type="fixed">
<origin xyz="${rplidar_offset_x} ${rplidar_offset_y} ${rplidar_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="laser_link"/>
</joint>
<xacro:usb_camera prefix="camera"/>
<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>
</robot>
新建zong.launch
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<param name="robot_description" command="$(find xacro)/xacro $(find myrobot)/urdf/zong.xacro" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 添加 Arbotix 节点 -->
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find myrobot)/config/fake_mrobot_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
</launch>
运行zong.launch
详细配置环境请查看上面camer.xacro文件的launch配置方法
roslaunch myrobot zong.launch
详细配置环境请查看上面camer.xacro文件的launch配置方法
五、搭建ArbotiX+rviz仿真环境
详细配置环境请查看上面camer.xacro文件的launch配置方法
因为我们的launch根据第一步都是有ArbotiX+rviz仿真环境的，所以任何一个launch都可以仿真
我们就以三个的为例子测试吧
mrobot_teleop--控制机器人功能包安装
下载mrobot_teleop.zip并放到自己工作空间下面并解压
链接: https://pan.baidu.com/s/1BEHK5RRY5oRjg67rgboYIQ?pwd=3q2g 提取码: 3q2g
caktin_make打开/home/xxx/(即~)下的.bashrc，并进行手动添加ros功能包mrobot_teleop的地址
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/xy/mrobot_teleopnzh修改为自己的工作文件夹
重启终端加载.bashrc文件
source ~/.bashrc进入mrobot_teleop的scripts文件夹,给python文件赋权限
chmod 777 mrobot_teleop.py运行zong.launch
详细配置环境请查看上面camer.xacro文件的launch配置方法
roslaunch myrobot zong.launch
详细配置环境请查看上面camer.xacro文件的launch配置方法
此时打开新终端运行
roslaunch mrobot_teleop mrobot_teleop.launchmrobot_teleop.launch报错处理
如果你出现这个报错
这个是因为你的ros目前默认的python是python3而mrobot_teleop里的mrobot_teleop.py默认是python2的程序
此时需要你修改python程序使得适应python3的法则
修正代码为
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
import sys
import tty
import termios
import select
msg = """
Control mrobot!
---------------------------
Moving around:
u i o
j k l
m , .
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
CTRL-C to quit
"""
moveBindings = {
'i':(1,0),
'o':(1,-1),
'j':(0,1),
'l':(0,-1),
'u':(1,1),
',':(-1,0),
'.':(-1,1),
'm':(-1,-1),
}
speedBindings = {
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
}
def getKey():
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
speed = .2
turn = 1
def vels(speed, turn):
return "currently:\tspeed %s\tturn %s " % (speed, turn)
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('mrobot_teleop')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
x = 0
th = 0
status = 0
count = 0
acc = 0.1
target_speed = 0
target_turn = 0
control_speed = 0
control_turn = 0
try:
print(msg)
print(vels(speed, turn))
while True:
key = getKey()
if key in moveBindings:
x = moveBindings[key][0]
th = moveBindings[key][1]
count = 0
elif key in speedBindings:
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
count = 0
print(vels(speed, turn))
if (status == 14):
print(msg)
status = (status + 1) % 15
elif key == ' ' or key == 'k':
x = 0
th = 0
control_speed = 0
control_turn = 0
else:
count += 1
if count > 4:
x = 0
th = 0
if (key == '\x03'):
break
target_speed = speed * x
target_turn = turn * th
if target_speed > control_speed:
control_speed = min(target_speed, control_speed + 0.02)
elif target_speed < control_speed:
control_speed = max(target_speed, control_speed - 0.02)
else:
control_speed = target_speed
if target_turn > control_turn:
control_turn = min(target_turn, control_turn + 0.1)
elif target_turn < control_turn:
control_turn = max(target_turn, control_turn - 0.1)
else:
control_turn = target_turn
twist = Twist()
twist.linear.x = control_speed
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = control_turn
pub.publish(twist)
except Exception as e:
print(e)
finally:
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
此时mrobot_teleop.launch正确运行
让我们按照上面提示进行按键确认机器人是否进行移动！
！！！！特别提醒！！！！
请确定Fixed Frame为 odom！
可以看到机器人已经移动了！
完整功能包：
链接: https://pan.baidu.com/s/1xdxeb1nFsVCUjtO-Q5CFrQ?pwd=47h5 提取码: 47h5
更多推荐
20
0- 0
已为社区贡献1条内容
所有评论(0)