目的很简单,能让Pepper从电梯口走到指定房间里。
大致上是参考的这篇文章
使用Pepper with ROS
但是这篇文章有细节上错的地方。
这里主要是纠正以下错误,并且给出安装中常见问题的解决方式
1.文章3.4步rosdep update
这里会有常见的time out报错我试了如下方法解决的
参考博客
然后多跑几次。
2.文章4.1步和4.2顺序反了
不得不说这一步的报错简直致命,我卡在这一步好久。
如果按照顺序会有如下报错,我一直去搜索如何解决依赖冲突,但是那是个死循环多亏了下面这篇博客根本不是没安装的问题!是没找到!
3.文章5.1步
运行roscore时候报错 Unable to contact my own server at的解决方法
4.文章5.2步
首先<>符号要换成单引号这是报错时我参考的博客。network_interface建议自己还是ifconfig命令查看以下,有的还真没在那篇文章选项里,比如我是wlp8s0。
5.文章6.2步
正常来说这里会报错是因为没有安装gmapping,可以自行搜素一下错误的解决方案。
6.得到了地图
7.打开地图
7.1 用ROS的命令roscd可以直接进入到文件夹catkin_ws
7.2 可以在文件夹里面打开文件也可以加入文件路径roslaunch pepper-ros-navigation navigation.launch map_file:=路径
但是目前存在一个问题,就是有的时候打开地图,机器人并没有在地图起始点。可能是某一步骤细节没注意到。目前还没有找到怎么把机器人拖动到地图某个位置的办法。
8.定点移动
由于楼道很长,ROS的路径规划算法是选择最优路线,走着走着就会很靠墙。pepper机器人防碰撞会在靠墙一定距离时选择停下,一旦停下就不会再走。
解决办法是在楼道地图上指定多个不靠墙的点,规定机器人移动必须经过这些点。
9.关于ROS的一些零碎但是有点用的知识
9.1 ROS喜欢称之为节点的东西其实就是进程
9.2 ROS目前支持的系统平台有: Ubuntu. osx.Debin。ROS在Ubuntu上的支持是最好的。建议内存4G或以上(内存太小,有些3D模拟可能运行不起来)。ROS是需要运行在Ubuntu操作系统之上,建议使用Ubuntu16.04和ROS Kinetic。理由是: ROS Kinetic 和Ubuntu 16.04都是长期维护版本(LTS),并且ROS Kinetic 是专门为Ubuntu16.04量身定做。
9.3
catkin_ws 是用户创建的工程文件目录(可以任意命名),包含整个工程;
src 目录是用户创建的功能包集合(不能修改名称),包含一个或多 个功能包;
build 目录是系统在编译src中的功能包时自动生成的,包含缓存信息和中间文件;
devel 目录是系统在编译src中的功能包时自动生成的,包含生 成的目标文件、环境变量。
这个pepper官方提供的包装在src文件夹相当于package 是用户创建的功能包
CMakeLists.txt——-package的编译规则(必须)
package.xml——-package的描述信息(必须)
launch/启动文件
include/C++头文件
src/源代码文件
scripts/可执行脚本
msg/自定义消息
srv/自定义服务
action/自定义action服务
urdf/urdf文件
10.详细代码
新建地图步骤
1.启动ros core节点管理器
roscore
2.在关闭自主模式下连接pepper
roslaunch pepper_bringup pepper_full.launch nao_ip:='192.168.43.64' roscore_ip:='127.0.0.1' network_interface:='wlp8s0'
3.启动rviz
rosrun rviz rviz -d ~/catkin_ws/src/pepper-ros-navigation/config/pepper.rviz
4.启动gmapping
roslaunch pepper-ros-navigation gmapping.launch
__(rostopic echo /move_base_simple/goal)
5.保存地图在某个路径cdros
rosrun map_server map_saver -f 文件名
6.打开你建的地图
roslaunch pepper-ros-navigation navigation.launch map_file:=路径
__(/home/elaine/catkin_ws/src/pepper-ros-navigation/launch/maps/map3.yaml)
定点巡逻:
1.启动ros core节点管理器
roscore
2.在关闭自主模式下连接pepper
roslaunch pepper_bringup pepper_full.launch nao_ip:='192.168.199.216' roscore_ip:='127.0.0.1' network_interface:='wlp8s0'
3.打开你建的地图
roslaunch pepper-ros-navigation navigation.launch map_file:=/home/elaine/catkin_ws/src/pepper-ros-navigation/launch/maps/map3.yaml
4.开始巡逻,设置定点巡逻次数为两次
roslaunch pepper-ros-navigation way_point.launch loopTimes:=2
way_point.launch文件
<launch>
<!-- For Simulation -->
<arg name="sim_mode" default="false" />
<param name="/use_sim_time" value="$(arg sim_mode)"/>
<arg name="loopTimes" default="0" />
<!-- move bas -->
<node pkg="pepper-ros-navigation" type="way_point.py" respawn="false" name="way_point" output="screen" launch-prefix="gnome-terminal -x">
<!-- params for move_base -->
<param name="goalListX" value="[1.2,0.1,1.1]" />
<param name="goalListY" value="[0.4,-1.0,-0.5]" />
<param name="goalListZ" value="[0.0,0.0,0.0]" />
<param name="loopTimes" value="$(arg loopTimes)" />
<param name="map_frame" value="map" />
</node>
</launch>
way_point.py脚本
#!/usr/bin/python
# coding=gbk
import rospy
import string
import math
import time
import sys
from std_msgs.msg import String
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped
import tf
class MultiGoals:
def __init__(self, goalListX, goalListY, goalListZ,loopTimes, map_frame):
self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)
self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
# params & variables
self.goalListX = goalListX
self.goalListY = goalListY
self.goalListZ = goalListZ
self.loopTimes = loopTimes
self.loop = 1
self.wayPointFinished = False
self.goalId = 0
self.goalMsg = PoseStamped()
self.goalMsg.header.frame_id = map_frame
self.goalMsg.pose.orientation.z = 0.0
self.goalMsg.pose.orientation.w = 1.0
# Publish the first goal
time.sleep(1)
self.goalMsg.header.stamp = rospy.Time.now()
self.goalMsg.pose.position.x = self.goalListX[self.goalId]
self.goalMsg.pose.position.y = self.goalListY[self.goalId]
self.goalMsg.pose.orientation.x = 0.0
self.goalMsg.pose.orientation.y = 0.0
if abs(self.goalListZ[self.goalId]) > 1.0:
self.goalMsg.pose.orientation.z = 0.0
self.goalMsg.pose.orientation.w = 1.0
else:
w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
self.goalMsg.pose.orientation.w = w
self.pub.publish(self.goalMsg)
rospy.loginfo("Current Goal ID is: %d", self.goalId)
self.goalId = self.goalId + 1
def statusCB(self, data):
if self.loopTimes and (self.loop > self.loopTimes):
rospy.loginfo("Loop: %d Times Finshed", self.loopTimes)
self.wayPointFinished = True
if data.status.status == 3 and (not self.wayPointFinished): # reached
self.goalMsg.header.stamp = rospy.Time.now()
self.goalMsg.pose.position.x = self.goalListX[self.goalId]
self.goalMsg.pose.position.y = self.goalListY[self.goalId]
if abs(self.goalListZ[self.goalId]) > 1.0:
self.goalMsg.pose.orientation.z = 0.0
self.goalMsg.pose.orientation.w = 1.0
else:
w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
self.goalMsg.pose.orientation.w = w
self.pub.publish(self.goalMsg)
rospy.loginfo("Current Goal ID is: %d", self.goalId)
if self.goalId < (len(self.goalListX)-1):
self.goalId = self.goalId + 1
else:
self.goalId = 0
self.loop += 1
if __name__ == "__main__":
try:
# ROS Init
rospy.init_node('way_point', anonymous=True)
# Get params
goalListX = rospy.get_param('~goalListX', '[2.0, 2.0]')
goalListY = rospy.get_param('~goalListY', '[2.0, 4.0]')
goalListZ = rospy.get_param('~goalListZ', '[0.0, 0.0]')
map_frame = rospy.get_param('~map_frame', 'map' )
loopTimes = int(rospy.get_param('~loopTimes', '0'))
goalListX = goalListX.replace("[","").replace("]","")
goalListY = goalListY.replace("[","").replace("]","")
goalListZ = goalListZ.replace("[","").replace("]","")
goalListX = [float(x) for x in goalListX.split(",")]
goalListY = [float(y) for y in goalListY.split(",")]
goalListZ = [float(z) for z in goalListZ.split(",")]
if len(goalListX) == len(goalListY) == len(goalListZ) & len(goalListY) >=2:
# Constract MultiGoals Obj
rospy.loginfo("Multi Goals Executing...")
mg = MultiGoals(goalListX, goalListY, goalListZ, loopTimes, map_frame)
rospy.spin()
else:
rospy.errinfo("Lengths of goal lists are not the same")
except KeyboardInterrupt:
print("shutting down")