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 ALLimport rospy, cv2, cv_bridge, numpyfrom sensor_msgs.msg import Imageclass 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 FILTERmasked = 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 ALLimport rospy, cv2, cv_bridge, numpyfrom sensor_msgs.msg import Imagefrom geometry_msgs.msg import Twistclass 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/4search_bot = 3*h/4 + 20mask[0:search_top, 0:w] = 0mask[search_bot:h, 0:w] = 0M = 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/2self.twist.linear.x = 0.2# 根据偏差值修改角速度大小，实现转向跟随self.twist.angular.z = -float(err) / 100self.cmd_vel_pub.publish(self.twist)# END CONTROLtry: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，如下图

结束啦！！！