def move_to_next_node(self):
print "moveto next node"
if len(self._poses_chain):
bug_goal = MoveToGoal()
bug_goal.target_pose = self._poses_chain.pop(0)
self._move_client.send_goal(bug_goal, done_cb=self.move_to_done_cb, feedback_cb=self.move_to_feedback_cb)
else:
return
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 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 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 button_send_cb(self):
x = self.x_spin.value()
y = self.y_spin.value()
yaw = self.yaw_spin.value()
goal = MoveToGoal()
q = quaternion_from_euler(0, 0, yaw * math.pi / 180)
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.client.send_goal(goal,
done_cb=self.action_done_cb,
active_cb=self.action_active_cb,
feedback_cb=self.action_feedback_cb)
goal_text = u'%.2f; %.2f; %i°' % (x, y, yaw)
self.update_state_text(goal_text)
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
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)