ROS导航仿真和多点导航3——实现机器人巡检功能
ROS导航仿真和多点导航3——实现机器人巡检功能
0.代码分享
1.主要结构梳理
2.任务初始化类class TaskInit
3.创建TaskTransfer对象，实现机器人从一个路径点到另一个路径点的功能
4.从当前文件中下载所有巡检点信息
5.将巡检点从RVIZ中的map中标记出来
5.巡检点TaskPoint类
从本地文件读取巡检点在map的位置，发布任务使机器人循环的在巡检点移动。
0.代码分享
链接：https://pan.baidu.com/s/1Fsi2sAUWse_Jb5sqy8ypUw
提取码：p9pe
1.主要结构梳理
class Task:
def __init__(self):
self.taskPoints = []
self.currentIndex = 0
self.robot_transfer = None
self.src_ind = None
self.des_ind = None
self.package_path = None
self.mark_pub = None
self.task_pose_pub = None
self.ntask = 0
#找到当前功能包的路径package_path
def init_rospack(self):
#get an instance of RosPack with the default search paths
rospack = rospkg.RosPack()
#list all packages,,equivalent to rospack list
rospack.list()
try:
self.package_path = rospack.get_path('auto_nav2d_point')
except:
print("Could not find package auto_nav2d_point")
exit(1)
print(self.package_path)
#初始化任务，，并将机器人由当前位置移动到距离最近的巡检点
def init(self):
self.mark_pub = rospy.Publisher('/robot_path',MarkerArray,queue_size = 100)
self.task_pose_pub = rospy.Publisher('/robot_path_pose',PoseArray,queue_size = 100)
task_init = TaskInit() #任务初始化类
self.init_rospack() #找到当前功能包的路径package_path
self.robot_transfer = TaskTransfer() #创建TaskTransfer对象，实现机器人从一个路径点到另一个路径点的功能
self.loadTaskPoints() #从当前文件中下载所有巡检点信息
#plot all task points
self.plot_marker(self.taskPoints) #将巡检点从RVIZ中的map中标记出来
best_ind,initial_point = task_init.getBestTaskInd(self.taskPoints) #找出距离机器人当前位置最小的巡检点
#First to transfer to best index point 'self.taskPoints[best_index]'
self.robot_transfer.task_transfer(initial_point,self.taskPoints[best_ind]) #将机器人从当前位置移到到最近的巡检点
self.taskPoints[best_ind].runTask() #到达巡检点后，执行事件
self.ntask = self.taskPoints.__len__() #循环到达下一个巡检点
self.src_ind = best_ind
self.des_ind = (best_ind + 1) % self.ntask
if __name__=="__main__":
rospy.init_node("auto_2d_nav_task")
task = Task() #class Task 任务类，用于发布巡检指令
task.init() #初始化任务，并将机器人由当前位置移动到距离最近的巡检点
task.run() #循环的在不同巡检点间顺序移动
2.任务初始化类class TaskInit
class TaskInit():
def __init__(self,pose_topic="/ndt/current_pose"):
self.initialPose = None
self.pose_topic = pose_topic
self.tf_listener = tf.TransformListener()
#通过base_link与map坐标的tf，得到机器人实时位置
def listen_tf(self):
try:
(pos,ori) = self.tf_listener.lookupTransform("/map","base_link",rospy.Duration(0.0))
print "pos:",pos
print "ori:",ori
msg_list = [
pos[0],pos[1],pos[2],
ori[0],ori[1],ori[2],ori[3]
]
self.initialPose = msg_list
return True
except tf.Exception as e:
print "listen to tf failed"
print e
print e.message
return False
#刷新位置，返回当前位置
def refreshInitialPose(self):
self.initialPose = None
RATE = 50
while not self.initialPose:
self.listen_tf() #对tf进行监听
rospy.sleep(1.0 / RATE)
#获得距离当前点最近的巡检点位置
def getBestTaskInd(self,task_points):
self.refreshInitialPose()
fake_task = TaskPoint()
fake_task.setRobotPose(self.initialPose)
dist_list = [fake_task.calDistance(task_point) for task_point in task_points]
return np.argmin(np.array(dist_list)),fake_task
3.创建TaskTransfer对象，实现机器人从一个路径点到另一个路径点的功能
#!/usr/bin/python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction,MoveBaseGoal
from TaskPoint import TaskPoint
from geometry_msgs.msg import PoseStamped
class TaskTransfer:
def __init__(self):
self.moveToClient = actionlib.SimpleActionClient('move_base',MoveBaseAction)
self.moveToClient.wait_for_server()
rospy.loginfo("Action 'MoveTo' is up !")
#将机器人从当前位置移到到最近的巡检点
def task_transfer(self,src_point,des_point):
'''
Transfer robot to next TaskPoint.
'''
des_point.setPreTaskPoint(src_point)
goal_msg = MoveBaseGoal()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = "map"
poseList = des_point.getPoseList()
goal_pose.pose.position.x = poseList[0]
goal_pose.pose.position.y = poseList[1]
goal_pose.pose.position.z = poseList[2]
goal_pose.pose.orientation.x =poseList[3]
goal_pose.pose.orientation.y = poseList[4]
goal_pose.pose.orientation.z = poseList[5]
goal_pose.pose.orientation.w = poseList[6]
goal_msg.target_pose = goal_pose
print "-----------------------------------------------"
print "goal_msg.target_pose.pose.position:",goal_msg.target_pose.pose.position
print "goal_msg.target_pose.pose.orientation:",goal_msg.target_pose.pose.orientation
print "--------------------------------------------------"
not_done = True
while not rospy.is_shutdown() and not_done:
self.moveToClient.send_goal(goal_msg)
rospy.logwarn("Transfer from [%s] to [%s]"%(src_point.name,des_point.name))
done = self.moveToClient.wait_for_result(timeout = rospy.Duration(300.0))
#Make sure the action succeed
not_done = (not done) or (self.moveToClient.gh.get_goal_status() != actionlib.GoalStatus.SUCCEEDED)
print "Goal status:" ,self.moveToClient.gh.get_goal_status == actionlib.GoalStatus.SUCCEEDED
if not_done:
print "Goal not done,Resent goal."
4.从当前文件中下载所有巡检点信息
从当前功能包的data目录下，读取机器人巡检点信息
def loadTaskPoints(self):
'''
Construct TaskPoint list from json files in data folder.
'''
folder = '{}/data'.format(self.package_path)
task_json = None
if os.path.exists(folder):
task_json=os.listdir(folder)
if not task_json:
raise Exception("No valid task point to tranverse!!")
task_list = []
for i,file_name in enumerate(task_json):
with open(folder + '/' + file_name,'r') as json_fp:
waypoint_record = json.load(json_fp)
task_list.append(waypoint_record)
task_list = sorted(task_list,key = lambda s : s['order'])
for waypoint_record in task_list:
self.taskPoints.append(
TaskPoint(waypoint_record)
)
5.将巡检点从RVIZ中的map中标记出来
从当前功能包的data目录下，读取机器人巡检点信息
def plot_marker(self,taskPoints):
markerArray = MarkerArray()
for point in taskPoints:
marker = Marker()
marker.header.frame_id = 'map'
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
marker.pose.position.x = point.getPosX()
marker.pose.position.y = point.getPosY()
marker.pose.position.z = point.getPosZ()
marker.text = str(point.getOrder())
markerArray.markers.append(marker)
id = 0
for m in markerArray.markers:
m.id = id
id += 1
6.巡检点TaskPoint类
#!/usr/bin/python
'''
Copyright (c) Deep Robotics Inc. - All Rights Reserved
Unauthorized copying of this file, via any medium is strictly prohibited
Proprietary and confidential
Author: Haoyi Han , Feb, 2020
'''
import time
from tf import transformations
import rospy
import tf
import copy
class TaskPoint:
"""Property and method about a task point.
Attributes:
pose: the pose of robot in this wappoint.
name: waypoint name.
"""
def __init__(self,record = None):
if not record:
record = {
"order" : 0,
"robot_pose" : {
"pos_x" : 0.0,
"pos_y" : 0.0,
"pos_z" : 0.0,
"ori_x" : 0.0,
"ori_y" : 0.0,
"ori_z" : 0.0,
"ori_w" : 1.0
}
}
self.record = copy.deepcopy(record)
self.tf_listener = tf.TransformListener()
self.update_waypoint_name()
def setPreTaskPoint(self,src_point):
self.pre_task_point = src_point
def getPreTaskPoint(self):
return self.pre_task_point
def update_waypoint_name(self):
self.name = "waypoint_" + str(self.record["order"])
def setRobotPose(self,robot_pose):
self.record["robot_pose"]["pos_x"] = robot_pose[0]
self.record["robot_pose"]["pos_y"] = robot_pose[1]
self.record["robot_pose"]["pos_z"] = robot_pose[2]
self.record["robot_pose"]["ori_x"] = robot_pose[3]
self.record["robot_pose"]["ori_y"] = robot_pose[4]
self.record["robot_pose"]["ori_z"] = robot_pose[5]
self.record["robot_pose"]["ori_w"] = robot_pose[6]
def getPosX(self):
robot_pose = self.record["robot_pose"]
return robot_pose["pos_x"]
def getPosY(self):
robot_pose = self.record["robot_pose"]
return robot_pose["pos_y"]
def getPosZ(self):
robot_pose = self.record["robot_pose"]
return robot_pose["pos_z"]
def getOrder(self):
return self.record["order"]
def getYaw(self):
robot_pose = self.record["robot_pose"]
pose = []
pose.append(robot_pose["pos_x"])
pose.append(robot_pose["pos_y"])
pose.append(robot_pose["pos_z"])
pose.append(robot_pose["ori_x"])
pose.append(robot_pose["ori_y"])
pose.append(robot_pose["ori_z"])
pose.append(robot_pose["ori_w"])
return transformations.euler_from_quaternion(pose[3:])[2]
def getPoseList(self):
robot_pose = self.record["robot_pose"]
pose = []
pose.append(robot_pose["pos_x"])
pose.append(robot_pose["pos_y"])
pose.append(robot_pose["pos_z"])
pose.append(robot_pose["ori_x"])
pose.append(robot_pose["ori_y"])
pose.append(robot_pose["ori_z"])
pose.append(robot_pose["ori_w"])
return pose
def calDistance(self,other):
'''
For Simplifacation, using 2D distance as the pose distance.
'''
return (
(self.getPosX()-other.getPosX())**2 + (self.getPosY()-other.getPosY())**2
)**0.5
def listen_tf(self):
try:
(pos,ori) = self.tf_listener.lookupTransform("/map","/base_link",rospy.Duration(0.0))
print "pos: ",pos
print "ori: ",ori
msg_list =[
pos[0],pos[1],pos[2],
ori[0],ori[1],ori[2],ori[3]
]
return msg_list
except tf.Exception as e:
print "listen to tf failed"
print e
print e.message
return None
#到达巡检点需要执行的操作定义
def runTask(self):
"""
When robot successfully tranfer to a new TaskPoint, Correspoing operation task will be triggered.
eg. shoot an image.
"""
rospy.loginfo("Run task in {}.".format(self.name))
rospy.sleep(3.0)