def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
def handle_path(self, paramExecutePathGoal):
'''
Action server callback to handle following path in succession
'''
rospy.loginfo('handle_path')
self.goal_index = 0
self.executePathGoal = paramExecutePathGoal
self.executePathResult = ExecutePathResult()
if self.executePathGoal is not None:
self.visualization_publisher.publish(self.executePathGoal.path)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
if not self.path_server.is_active():
return
if self.path_server.is_preempt_requested():
rospy.loginfo('Preempt requested...')
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_preempted()
return
if self.goal_index < len(self.executePathGoal.path.poses):
moveto_goal = MoveToGoal()
moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
feedback_cb=self.handle_goal_preempt)
# Wait for result
while self.goal_client.get_result() is None:
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
else:
rospy.loginfo('Done processing goals')
self.goal_client.cancel_goal()
self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
return
rate.sleep()
self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')
def execute_cb(self, goal):
rospy.loginfo(len(goal.path.poses))
if self.use_obstacle_avoidance:
SERVER = '/bug2/move_to'
else:
SERVER = '/motion_controller/move_to'
#after receiving the path, publish the path
self.path_publisher.publish(goal.path)
#creating the client to send each pose to the server
self.client = SimpleActionClient(SERVER, MoveToAction)
self.client.wait_for_server()
#passing each pose to the server and handling the result from the server
for pose in goal.path.poses:
self.goal_to_move_to = MoveToGoal()
self.feedback = ExecutePathFeedback()
self.visited = ExecutePathResult()
self.goal_to_move_to.target_pose = pose
self.client.send_goal(self.goal_to_move_to,
done_cb=self.move_to_done_cb)
rospy.logwarn("goal passed to motion_controller")
self.client.wait_for_result()
rospy.loginfo('Before feedback pose')
self._move_to_server.set_succeeded(self.visited)
#client.get_result()
pass
class PathExecutor:
_continue=True
_skip_unreachable=True
_path_result=ExecutePathResult()
def __init__(self):
self._poses = []
self._poses_idx = 0
self._client = SimpleActionClient('/bug2/move_to', MoveToAction)
self._action_name = rospy.get_name() + "/execute_path"
self._server = SimpleActionServer(self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start = False)
self._server.start()
def execute_cb(self, goal):
"""
This funciton is used to execute the move to goal
"""
self._path_result.visited=[] #Initialize the path result
self._skip_unreachable=goal.skip_unreachable
self._client.wait_for_server()
self._poses = goal.path.poses
self._poses_idx = 0 #Start with the first goal
self._continue= True
rospy.loginfo('Starting Number of Poses: %s'%len(self._poses))
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
"""
Send the goal to the _client and once done, go to callback function
"move_to_done_cb"
"""
self._client.send_goal(move, done_cb=self.move_to_done_cb)
while (self._continue==True):
"""
Check if current goal is preempted by other user action.
If so, break out of loop
"""
if self._server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._server.set_preempted()
self._continue= False
break
def move_to_done_cb(self, state, result):
"""
This call back function is called once one goal is completed.
This checks if there are any other goal remaining to be executed and
recurrsively calls itself.
"""
feedback = ExecutePathFeedback()
feedback.pose=self._poses[self._poses_idx]
"""
#======================================================================
#* If state is 3, i.e., goal was successful, publish feedback
# and move to next goal
#
#* Else publish feedback and check _skip_unreachable flag,
# * If True go to next pose
# * Else break
#======================================================================
"""
if(state == 3):
feedback.reached = True
self._path_result.visited.append(feedback.reached)
self._server.publish_feedback(feedback)
self._poses_idx = self._poses_idx + 1
"""
* If more pose available, move to next goal
* Else break
"""
if(self._poses_idx < len(self._poses)):
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
self._client.send_goal(move, done_cb=self.move_to_done_cb)
else:
self._continue=False
self._server.set_succeeded(self._path_result)
else:
feedback.reached = False
self._path_result.visited.append(False)
self._server.publish_feedback(feedback)
if(self._skip_unreachable==True):
self._poses_idx = self._poses_idx + 1
"""
* If more pose available, move to next goal
* Else break
"""
if(self._poses_idx < len(self._poses)):
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
self._client.send_goal(move, done_cb=self.move_to_done_cb)
else:
self._continue=False
self._server.set_succeeded(self._path_result)
else:
rospy.loginfo('Unreachable')
self._continue=False
self._server.set_succeeded(self._path_result)
class PathExecutor:
_feedback = ExecutePathFeedback()
_result = ExecutePathResult()
def __init__(self, name):
# self._goal_publisher = Simple
# self.pe_client_for_bug2= SimpleActionClient('/bug2/move_to',MoveToAction)
# self.pe_client_for_bug2.wait_for_server()
print "----------------- in INIT ---------------------------------------"
rospy.loginfo("Creating an action server")
# Creating a client to perform the bug2 function
self._move_to_client = SimpleActionClient('/bug2/move_to',
MoveToAction)
# Creating an action server
self._as = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, \
execute_cb=self.execute_cb, auto_start=False)
# Starting the server
self._as.start()
# Using a flag to aid preemption
self.flag = True
# Using a flag to detect the completion of 3 goals
self.success = True
def execute_cb(self, goal):
# Executed every time a goal is sent by the client
self._move_to_client.wait_for_server()
# printing the goals in the terminal
rospy.loginfo(goal.path.poses)
# Creating an instance of MoveToGoal to compose a goal message
self.move = MoveToGoal()
# Iterating through the goals and sending it to the move_to_client
for i in range(len(goal.path.poses)):
if i == 2:
self.success = True
else:
self.success = False
self._pose = goal.path.poses[i]
self.move.target_pose = self._pose
self._move_to_client.send_goal(self.move,
done_cb=self.move_to_done_cb)
self._move_to_client.wait_for_result()
# self.success = True
rospy.spin()
# setting the server's preempted() function if it's requested
while flag:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
break
def move_to_done_cb(self, state, result):
if (state == 3):
#
self._result.visited.append(True)
self._feedback.reached = True
self._feedback.pose = self._pose
elif (state == 4):
self._feedback.reached = False
#
rospy.loginfo(self._result)
self._as.publish_feedback(self._feedback)
if self.success == True:
print "All goals are considered"
self._as.set_succeeded(self._result)
def execute_cb(self, goal):
rospy.loginfo("ExecutePathAction received")
# create messages that are used to publish feedback/result
_feedback = ExecutePathFeedback()
_result = ExecutePathResult()
r = rospy.Rate(10)
success = True
aborted = False
goal2 = MoveToAction
#Get obstacle avoidance
self.obstacle_avoidance = rospy.get_param(
'/path_executor/use_obstacle_avoidance')
#Choose server for move_to depending on obstacle avoidance
if self.obstacle_avoidance:
rospy.loginfo("obstacle_avoidance = True")
move_client = SimpleActionClient('/bug2/move_to', MoveToAction)
else:
rospy.loginfo("obstacle_avoidance = False")
move_client = SimpleActionClient('/motion_controller/move_to',
MoveToAction)
move_client.wait_for_server()
#Publish path to /path_executor/current_path
self.publisher.publish(goal.path)
#Iterate through path
for index in range(len(goal.path.poses)):
rospy.loginfo("\nGoal received")
rospy.loginfo("Goal: \n{0}".format(goal.path.poses[index].pose))
#Publish goal pose to move_to server
goal2.target_pose = goal.path.poses[index]
move_client.send_goal(goal2)
rospy.loginfo("Sending goal to move_to action server ...")
while not rospy.is_shutdown():
#Check for preemption
if self._as.is_preempt_requested():
rospy.logwarn('Preempted requested')
move_client.cancel_all_goals()
self._as.set_preempted()
success = False
aborted = True
break
#get result state from move_to server
state = move_client.get_state()
#Success
if state == 3:
rospy.loginfo("Goal reached")
#Publish the feedback
_feedback.pose = goal2.target_pose
_feedback.reached = True
self._as.publish_feedback(_feedback)
#Store to visited poses
_result.visited.append(True)
break
#Unreachable goal
elif state == 4:
rospy.logwarn("Goal is unreachable")
#Skip unreachable
if goal.skip_unreachable:
rospy.logwarn("Skipping unreachable goal")
#Publish the feedback
_feedback.pose = goal2.target_pose
_feedback.reached = False
self._as.publish_feedback(_feedback)
#Store visited poses
_result.visited.append(False)
break
#Abort
else:
rospy.logwarn("Abort unreachable goal")
rospy.loginfo("Cancelling goals...")
move_client.cancel_all_goals()
rospy.logwarn("Goals cancelled")
rospy.loginfo("Aborting action server")
self._as.set_aborted()
rospy.logwarn("Action server aborted")
success = False
aborted = True
break
r.sleep()
if aborted:
break
if success:
self._as.set_succeeded(_result)
rospy.loginfo("Path finished with success\n")