Gazebo-Ros搭建小车和场景并运行slam算法进行建图4–为机器人添加运动控制器控制其移动
1.要想机器人小车在gazebo中运动还需要为其添加运动插件
在文章3中的my_robot2.urdf 最下边（前边）添加如下部分：
这里使用的二轮差速控制，选择对应的插件libgazebo_ros_diff_drive.so
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<rosDebuglevel>Debug</rosDebuglevel>
<publishWheelTF>false</publishWheelTF>
<publishTF>1</publishTF>
<publishWheelJointState>false</publishWheelJointState>
<updateRate>100.0</updateRate>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<wheelSeparation>0.4</wheelSeparation>
<wheelDiameter>0.12</wheelDiameter>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<robotBaseFrame>base_footprint</robotBaseFrame>
<odometryFrame>odom</odometryFrame>
<broadcastTF>1</broadcastTF>
</plugin>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/my_robot</robotNamespace>
</plugin>
</gazebo>
其中
<wheelSeparation>0.4</wheelSeparation> 这个是轮距
<wheelDiameter>0.12</wheelDiameter> 这个是轮子直径，根据机器人模型的实际参数设置
添加上述部分，文章3中的my_robot2.urdf 更新为 --完整代码如下：
<?xml version="1.0" ?>
<robot name="my_robot">
<link name="base_link">
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<geometry>
<cylinder length="0.16" radius="0.20" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<geometry>
<cylinder length="0.16" radius="0.20" />
</geometry>
<material name="orange">
<color rgba="1 0.4 0 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="6" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<gazebo reference="base_link">
<material>Gazebo/Orange</material>
</gazebo>
<joint name="left_wheel_joint" type="continuous">
<origin xyz="0 0.19 -0.05" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="left_wheel_link">
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0" />
<geometry>
<cylinder radius="0.06" length = "0.025"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0" />
<geometry>
<cylinder radius="0.06" length = "0.025"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<joint name="right_wheel_joint" type="continuous">
<origin xyz="0 -0.19 -0.05" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="right_wheel_link">
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0" />
<geometry>
<cylinder radius="0.06" length = "0.025"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0" />
<geometry>
<cylinder radius="0.06" length = "0.025"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<joint name="front_caster_joint" type="continuous">
<origin xyz="0.18 0 -0.095" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="front_caster_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="front_caster_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.015" />
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<joint name="back_caster_joint" type="continuous">
<origin xyz="-0.18 0 -0.095" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="back_caster_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="back_caster_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.015" />
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<link name="camera_link">
<collision>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.02 0.05 0.03" />
</geometry>
</collision>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.02 0.05 0.03" />
</geometry>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<gazebo reference="camera_link">
<material>Gazebo/Black</material>
<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>
<joint name="camera_joint" type="fixed">
<origin xyz="0.17 0 0.095" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="camera_link"/>
</joint>
<link name="laser_link">
<collision>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.05"/>
</geometry>
</collision>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<gazebo reference="laser_link">
<material>Gazebo/Black</material>
<sensor type="gpu_ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for Hokuyo laser
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
reading. -->
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">
<topicName>/scan</topicName>
<frameName>laser_link</frameName>
</plugin>
</sensor>
</gazebo>
<joint name="laser_joint" type="fixed">
<origin xyz="0 0 0.105" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="laser_link"/>
</joint>
<link name="imu_link">
<collision>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.02 0.02 0.01" />
</geometry>
</collision>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.02 0.02 0.01" />
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.5" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>10.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
<initialOrientationAsReference>false</initialOrientationAsReference>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<joint name="imu_joint" type="fixed">
<origin xyz="0.1 0 0.085" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<rosDebuglevel>Debug</rosDebuglevel>
<publishWheelTF>false</publishWheelTF>
<publishTF>1</publishTF>
<publishWheelJointState>false</publishWheelJointState>
<updateRate>100.0</updateRate>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<wheelSeparation>0.4</wheelSeparation>
<wheelDiameter>0.12</wheelDiameter>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<robotBaseFrame>base_footprint</robotBaseFrame>
<odometryFrame>odom</odometryFrame>
<broadcastTF>1</broadcastTF>
</plugin>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/my_robot</robotNamespace>
</plugin>
</gazebo>
</robot>
2.使用turtlebot3中的控制机器人的文件
2.1在src目录下创建teleop_robot功能包，并添加依赖rospy geometry_msgs
$ cd ~/Documents/test_ws/src
$ catkin_create_pkg teleop_robot rospy geometry_msgs
2.2 在teleop_robot/src文件夹下创建文件teleop_robot_key.py
$ cd src/teleop_robot/src/
$ gedit teleop_robot_key
输入以下内容：
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
if os.name == 'nt':
import msvcrt, time
else:
import tty, termios
BURGER_MAX_LIN_VEL = 0.32
BURGER_MAX_ANG_VEL = 2.84
WAFFLE_MAX_LIN_VEL = 0.26
WAFFLE_MAX_ANG_VEL = 1.82
LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.1
msg = """
Control Your TurtleBot3!
---------------------------
Moving around:
w
a s d
x
w/x : increase/decrease linear velocity (Burger : ~ 0.32, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)
space key, s : force stop
CTRL-C to quit
"""
e = """
Communications Failed
"""
def getKey():
if os.name == 'nt':
timeout = 0.1
startTime = time.time()
while(1):
if msvcrt.kbhit():
if sys.version_info[0] >= 3:
return msvcrt.getch().decode()
else:
return msvcrt.getch()
elif time.time() - startTime > timeout:
return ''
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
def vels(target_linear_vel, target_angular_vel):
return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)
def makeSimpleProfile(output, input, slop):
if input > output:
output = min( input, output + slop )
elif input < output:
output = max( input, output - slop )
else:
output = input
return output
def constrain(input, low, high):
if input < low:
input = low
elif input > high:
input = high
else:
input = input
return input
def checkLinearLimitVelocity(vel):
if turtlebot3_model == "burger":
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
else:
vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
return vel
def checkAngularLimitVelocity(vel):
if turtlebot3_model == "burger":
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
else:
vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
return vel
if __name__=="__main__":
if os.name != 'nt':
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('turtlebot3_teleop')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
turtlebot3_model = rospy.get_param("model", "burger")
status = 0
target_linear_vel = 0.0
target_angular_vel = 0.0
control_linear_vel = 0.0
control_angular_vel = 0.0
try:
print(msg)
while not rospy.is_shutdown():
key = getKey()
if key == 'w' :
target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'x' :
target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'a' :
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'd' :
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == ' ' or key == 's' :
target_linear_vel = 0.0
control_linear_vel = 0.0
target_angular_vel = 0.0
control_angular_vel = 0.0
print(vels(target_linear_vel, target_angular_vel))
else:
if (key == '\x03'):
break
if status == 20 :
print(msg)
status = 0
twist = Twist()
control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
pub.publish(twist)
except:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
pub.publish(twist)
if os.name != 'nt':
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
3.进入工作空间根目录进行编译
$ cd ~/Documents/test_ws
$ catkin_make
$ source devel/setup.bash
$ rospack find teleop_robot (编译好的话可以找到这个功能包的所在路径)
4.开启三个终端测试控制效果
进入终端进入到~/Documents/test_ws后需要执行一下
$ source devel/setup.bash
然后分别在三个终端运行以下三个命令：
1.打开gazebo
$ roslaunch myrobot_description test.launch
2.打开rviz
$ roslaunch myrobot_description display_my_robot.launch
3.运行控制节点，根据提示可以改变线速度和角速度让机器人动起来
$ rosrun teleop_robot teleop_robot_key
5.接下来就是运行slam算法了
主要就是slam算法订阅的传感器主题要和机器人发布的传感器一致，这里是/scan，还有坐标系，这里雷达的坐标系是 laser_link .
3623
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
