def send_points(self, points):
# self.get_logger().info('Waiting for action server...')
msg = FollowWaypoints.Goal()
msg.poses = points
self._client.wait_for_server()
self._send_goal_future = self._client.send_goal_async(msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
self.get_logger().info('Sending goal request...')
def run(self, block):
if not self.waypoints:
rclpy.error_msg('Did not set valid waypoints before running test!')
return False
while not self.action_client.wait_for_server(timeout_sec=1.0):
self.info_msg(
"'follow_waypoints' action server not available, waiting...")
action_request = FollowWaypoints.Goal()
action_request.poses = self.waypoints
self.info_msg('Sending goal request...')
send_goal_future = self.action_client.send_goal_async(action_request)
try:
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e, ))
if not self.goal_handle.accepted:
self.error_msg('Goal rejected')
return False
self.info_msg('Goal accepted')
if not block:
return True
get_result_future = self.goal_handle.get_result_async()
self.info_msg("Waiting for 'follow_waypoints' action to complete")
try:
rclpy.spin_until_future_complete(self, get_result_future)
status = get_result_future.result().status
result = get_result_future.result().result
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e, ))
if status != GoalStatus.STATUS_SUCCEEDED:
self.info_msg('Goal failed with status code: {0}'.format(status))
return False
if len(result.missed_waypoints) > 0:
self.info_msg('Goal failed to process all waypoints,'
' missed {0} wps.'.format(
len(result.missed_waypoints)))
return False
self.info_msg('Goal succeeded!')
return True
def moveToFrontiers(self):
frontiers = getFrontier(self.currentPose, self.costmap, self.get_logger())
if len(frontiers) == 0:
self.info_msg('No More Frontiers')
return
location = None
largestDist = 0
for f in frontiers:
dist = math.sqrt(((f[0] - self.currentPose.position.x)**2) + ((f[1] - self.currentPose.position.y)**2))
if dist > largestDist:
largestDist = dist
location = [f]
#worldFrontiers = [self.costmap.mapToWorld(f[0], f[1]) for f in frontiers]
self.info_msg(f'World points {location}')
self.setWaypoints(location)
action_request = FollowWaypoints.Goal()
action_request.poses = self.waypoints
self.info_msg('Sending goal request...')
send_goal_future = self.action_client.send_goal_async(action_request)
try:
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
except Exception as e:
self.error_msg('Service call failed %r' % (e,))
if not self.goal_handle.accepted:
self.error_msg('Goal rejected')
return
self.info_msg('Goal accepted')
get_result_future = self.goal_handle.get_result_async()
self.info_msg("Waiting for 'FollowWaypoints' action to complete")
try:
rclpy.spin_until_future_complete(self, get_result_future)
status = get_result_future.result().status
result = get_result_future.result().result
except Exception as e:
self.error_msg('Service call failed %r' % (e,))
#self.currentPose = self.waypoints[len(self.waypoints) - 1].pose
self.moveToFrontiers()
def followWaypoints(self, poses):
"""Send a `FollowWaypoints` action request."""
self.debug("Waiting for 'FollowWaypoints' action server")
while not self.follow_waypoints_client.wait_for_server(
timeout_sec=1.0):
self.info(
"'FollowWaypoints' action server not available, waiting...")
goal_msg = FollowWaypoints.Goal()
goal_msg.poses = poses
self.info(f'Following {len(goal_msg.poses)} goals....')
send_goal_future = self.follow_waypoints_client.send_goal_async(
goal_msg, self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error(
f'Following {len(poses)} waypoints request was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True