ubuntu16.04ros-用海龟机器人仿真循线系统

下载安装sudo apt-get install ros-kinetic-turtlebot ros-kinetic-turtlebot-apps ros-kinetic-turtlebot-interactions ros-kinetic-turtlebot-simulator ros-kinetic-kobuki-ftdi

sudo apt-get install ros-kinetic-rocon-*

echo "source /opt/ros/kinetic/setup.bash" >>~/.bashrc
source ~/.bashrc
rosdep update

1进入home目录，按下ctrl+h显示隐藏文件。进入.gezebo/models文件夹，将附件models压缩包解压到这个文件夹下
发现我的.gezebo目录下没有models文件，分析原因后
首先安装Git

sudo apt-get update
sudo apt-get install git

然后使用git clone命令克隆gazebo_models仓库到.gezebo/models目录。需要先创建：

mkdir -p ~/.gazebo/models
git clone https://github.com/osrf/gazebo_models.git ~/.gazebo/models

安装OpenNI相机驱动包和OpenNI启动包

sudo apt-get install ros-kinetic-openni-camera
sudo apt-get install ros-kinetic-openni-launch

启动 roscore 进程 roscore
启动仿真环境

roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=/opt/ros/kinetic/share/turtlebot_gazebo/worlds/playground.world

启动传感器

rostopic echo scan -n 1

加载世界地图roslaunch turtlebot_gazebo turtlebot_world.launch
在rviz中显示

roslaunch turtlebot_rviz_launchers view_robot.launch --screen

在左侧选择显示深度相机数据DepthCloud,显示深度图片Image,在Image中选择rgb话题加载图片

设计循线路径
创建如下文件夹

将下列图片MyImage.png放进模型my_ground_plane/materials/textures下

在my_ground_plane文件下创建model.sdf文件和model.config文件，内容如下

model.config内容

<?xml version="1.0" encoding="UTF-8"?>
<model>
   <name>My Ground Plane</name>
   <version>1.0</version>
   <sdf version="1.4">model.sdf</sdf>
   <description>My textured ground plane.</decsription>
</model>

model.sdf内容如下

<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.4">
   <model name="my_ground_plane">
      <static>true</static>
      <link name="link">
	 <collision name="collision">
	    <geometry>
	       <plane>
		  <normal>0 0 1</normal>
		  <size>15 15</size>
	       </plane>
	    </geometry>
	    <surface>
	        <friction>
		   <ode>
		      <mu>100</mu>
		      <mu2>50</mu2>
		   </ode>
	        </friction>
	    </surface>
	 </collision>
	 <visual name="visual">
	    <cast_shadows>false</cast_shadows>
	    <geometry>
	       <plane>
		  <normal>0 0 1</normal>
		  <size>15 15</size>
	       </plane>
	    </geometry>
	    <material>
	       <script>
		  <uri>model://my_ground_plane/materials/scripts</uri>
	          <uri>model://my_ground_plane/materials/textures/</uri>
	          <name>MyGroundPlane/Image</name>
	       </script>
	    </material>
	 </visual>
      </link>
   </model>
</sdf>
	

然后打开gazebo

roslaunch turtlebot_gazebo turtlebot_world.launch

另开一个终端roslaunch turtlebot_rviz_launchers view_robot.launch --screen
点击gazebo左上角的insert插入刚建立的my_ground_plane模型
但是发现gazebo内没有刚建立的模型
可能是Gazebo未更新模型路径

export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:/你模型的路径/模型名

要使这个变量在每次打开新终端时都可用，添加

nano ~/.bashrc

添加上述 export 命令到文件的末尾，保存并关闭文件。然后重新加载.bashrc文件

source ~/.bashrc

验证环境变量

echo $GAZEBO_MODEL_PATH

发现有刚才添加的模型路径
重新启动gazebo
发现还是没有，于是试一下：模型文件夹位于ROS的工作空间中
我的工作空间命名为catkin_ws
首先将模型文件复制进工作空间的src下

cp -r /home/hh/.gazebo/models/my_ground_plane ~/catkin_ws/src/my_ground_plane

然后更新工作空间并重新编译

cd ~/catkin_ws
catkin_make

然后源更新后的工作空间，以便ROS能够识别新添加的模型

source devel/setup.bash

还是不行，最后我找到了一种直接放进去的办法，看下面
使用spawn_model服务来加载和放置模型

rosrun gazebo_ros spawn_model -file ~/catkin_ws/src/my_ground_plane/model.sdf -sdf -model my_ground_plane

点中物品右键删除，除了导入的地板和机器人

写脚本控制机器人循线

cd src/turtlrbot1/src
gedit follower_color_filter.py
rosrun turtlebot1 follower_color_filter.py

follower_color_filter.py如下：

#!/usr/bin/env python
 
# BEGIN ALL
 
import rospy, cv2, cv_bridge, numpy
 
from sensor_msgs.msg import Image
 
 
 
class Follower:
 
  def __init__(self):
 
    self.bridge = cv_bridge.CvBridge()
 
    self.image_sub = rospy.Subscriber('camera/rgb/image_raw',
 
                                      Image, self.image_callback)
 
    self.ori_pub = rospy.Publisher('ori', Image, queue_size=1)
 
    self.hsv_pub = rospy.Publisher('hsv', Image, queue_size=1)
 
    self.mask_pub = rospy.Publisher('mask', Image, queue_size=1)
 
  def image_callback(self, msg):
 
    # BEGIN BRIDGE
 
    # 将摄像头采集到的sensor_msgs/Image消息转换为OpenCV使用的对象
 
    # 将原始sensor_msgs/Image消息发布，用于RViz显示
 
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
 
    self.ori_pub.publish(msg)
 
    # END BRIDGE
 
    # BEGIN HSV
 
    # 将RGB图像转换为HSV图像
 
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
 
    # 将HSV图像转换为sensor_msgs/Image消息并发布
 
    try:
 
       self.hsv_pub.publish(self.bridge.cv2_to_imgmsg(hsv))
 
    except cv_bridge.CvBridgeError as e:
 
       print(e)
 
    # END HSV
 
    # BEGIN FILTER
 
    # 使用黄色阈值判断色调，转换为二值图
 
    lower_yellow = numpy.array([ 26,  43, 46])
 
    upper_yellow = numpy.array([34, 255, 255])
 
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
 
    # END FILTER
 
    masked = cv2.bitwise_and(image, image, mask=mask)
 
    # 发布mask消息
 
    try:
 
       self.mask_pub.publish(self.bridge.cv2_to_imgmsg(mask))
 
    except cv_bridge.CvBridgeError as e:
 
       print(e)
 
    cv2.waitKey(3)
 
 
 
rospy.init_node('follower')
 
follower = Follower()
 
rospy.spin()
 
# END ALL

运行时报错了 SyntaxError: Non-ASCII character '\xe5' in file /home/hh/turtlebot_ws/src/turtlrbot1/src/follower_color_filter.py on line 31, but no encoding declared; see http://python.org/dev/peps/pep-0263/ for details
在代码开头加上一句 # -*- coding: utf-8 -*- 就可以啦
输入rostopic list查看是否有ori、hsv、mask话题，打开RViz

rosrun rviz rviz

点击界面左下角add按钮，在弹窗顶部选择by topic选项卡，分别点击/ori、/hsv、/mask下的image，添加image话题后，显示如下：

关闭这个后，编写运行follower_line.py

#!/usr/bin/env python
 
# BEGIN ALL
 
import rospy, cv2, cv_bridge, numpy
 
from sensor_msgs.msg import Image
 
from geometry_msgs.msg import Twist
 
 
 
class Follower:
 
  def __init__(self):
 
    self.bridge = cv_bridge.CvBridge()
 
    self.image_sub = rospy.Subscriber('camera/rgb/image_raw',
 
                                      Image, self.image_callback)
 
    self.cmd_vel_pub = rospy.Publisher('cmd_vel',
 
                                       Twist, queue_size=1)
 
    self.circle_pub = rospy.Publisher('circle', Image, queue_size=1)
 
    self.twist = Twist()
 
  def image_callback(self, msg):
 
    # 检测黄色线条
 
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
 
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
 
    lower_yellow = numpy.array([ 26,  43, 46])
 
    upper_yellow = numpy.array([34, 255, 255])
 
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
 
    # 获取图像尺寸信息：高度、宽度、通道数
 
    h, w, d = image.shape
 
    # 限定关注范围，检测边线
 
    search_top = 3*h/4
 
    search_bot = 3*h/4 + 20
 
    mask[0:search_top, 0:w] = 0
 
    mask[search_bot:h, 0:w] = 0
 
    M = cv2.moments(mask)
 
    if M['m00'] > 0:
 
      cx = int(M['m10']/M['m00'])
 
      cy = int(M['m01']/M['m00'])
 
      # 绘制目标点
 
      cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
 
      # BEGIN CONTROL
 
      # 计算中心偏差值
 
      err = cx - w/2
 
      self.twist.linear.x = 0.2
 
      # 根据偏差值修改角速度大小，实现转向跟随
 
      self.twist.angular.z = -float(err) / 100
 
      self.cmd_vel_pub.publish(self.twist)
 
      # END CONTROL
 
    try:
 
       self.circle_pub.publish(self.bridge.cv2_to_imgmsg(image))
 
    except cv_bridge.CvBridgeError as e:
 
       print(e)
 
cv2.waitKey(3)
 
 
 
rospy.init_node('follower')
 
follower = Follower()
 
rospy.spin()
 
# END ALL

运行巡线脚本

rosrun turtlebot1 follower_line.py

打开rviz
如上点开circle话题里的Image，如下图

结束啦！！！