mirror of
https://github.com/Jieshoudaxue/ros_senior.git
synced 2026-08-29 08:34:43 +08:00
jieshoudaxue
78 lines
2.6 KiB
Python
Executable File
78 lines
2.6 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
import rospy
|
|
import actionlib
|
|
from actionlib_msgs.msg import *
|
|
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
|
|
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
|
|
from std_msgs.msg import Int8
|
|
from functools import partial
|
|
|
|
STATUS_EXPLORING = 0
|
|
STATUS_CLOSE_TARGET = 1
|
|
STATUS_GO_HOME = 2
|
|
|
|
target_list = []
|
|
target_list.append(Pose(Point(0, 8, 0), Quaternion(0, 0, 0, 1.0)))
|
|
target_list.append(Pose(Point(8, 8, 0), Quaternion(0, 0, 0, 1.0)))
|
|
target_list.append(Pose(Point(8, 0, 0), Quaternion(0, 0, 0, 1.0)))
|
|
target_list.append(Pose(Point(2.5, 4.8, 0), Quaternion(0, 0, 0, 1.0)))
|
|
target_list.append(Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1.0)))
|
|
|
|
def state_cb(state, move_base_client):
|
|
rospy.loginfo("receive state is %d" state.data)
|
|
|
|
if state.data == STATUS_CLOSE_TARGET:
|
|
move_base_client.cancel_goal()
|
|
rospy.loginfo("stop exploring")
|
|
elif state.data == STATUS_GO_HOME:
|
|
goal = MoveBaseGoal()
|
|
goal.target_pose.pose = target_list[-1]
|
|
goal.target_pose.header.frame_id = 'map'
|
|
goal.target_pose.header.stamp = rospy.Time.now()
|
|
|
|
move_base_client.send_goal(goal)
|
|
move_base_client.wait_for_result(rospy.Duration(300))
|
|
if move_base_client.get_state() == GoalStatus.SUCCEEDED:
|
|
rospy.loginfo("go home successed")
|
|
|
|
|
|
def main():
|
|
rospy.init_node("exploring_maze", anonymous=True)
|
|
move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
|
|
|
|
rospy.loginfo("Waiting for move_base action server...")
|
|
|
|
while move_base_client.wait_for_server(rospy.Duration(5.0)) == 0:
|
|
rospy.loginfo("connected to move base server")
|
|
|
|
bind_state_cb = partial(state_cb, move_base_client=move_base_client)
|
|
rospy.Subscriber("/state_cmd", Int8, bind_state_cb)
|
|
|
|
for i, target in enumerate(target_list):
|
|
start_time = rospy.Time.now()
|
|
|
|
goal = MoveBaseGoal()
|
|
goal.target_pose.pose = target
|
|
goal.target_pose.header.frame_id = 'map'
|
|
goal.target_pose.header.stamp = rospy.Time.now()
|
|
|
|
rospy.loginfo("going to {0} goal, {1}".format(i, str(target)))
|
|
|
|
move_base_client.send_goal(goal)
|
|
|
|
finished_within_time = move_base_client.wait_for_result(rospy.Duration(300))
|
|
|
|
if not finished_within_time:
|
|
move_base_client.cancel_goal()
|
|
rospy.loginfo("time out, failed to goal")
|
|
else:
|
|
running_time = (rospy.Time.now() - start_time).to_sec()
|
|
if move_base_client.get_state() == GoalStatus.SUCCEEDED:
|
|
rospy.loginfo("go to {0} goal succeeded, run time: {1} sec".format(i, running_time))
|
|
else:
|
|
rospy.loginfo("goal failed")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main() |