def sendGoalToClient(self, posX, posY, yaw = 0):
self.goal = NavigateToPose.Goal()
self.goal.pose.header.frame_id = "map"
# self.goal.pose.header.stamp = self._node.get_clock().now().to_msg()
self.goal.pose.pose.position.x = float(posX)
self.goal.pose.pose.position.y = float(posY)
print("Got Goal!")
print(self.goal)
# orientation_q = quaternion_from_euler(0, 0, yaw)
self.goal.pose.pose.orientation.x = 0.0
self.goal.pose.pose.orientation.y = 0.0
self.goal.pose.pose.orientation.z = 0.0
self.goal.pose.pose.orientation.w = 1.0
# self.goal.target_pose.pose.orientation.x = orientation_q[0]
# self.goal.target_pose.pose.orientation.y = orientation_q[1]
# self.goal.target_pose.pose.orientation.z = orientation_q[2]
# self.goal.target_pose.pose.orientation.w = orientation_q[3]
self._action_client.wait_for_server()
self._action_client.send_goal_async(self.goal)
# self.data = [0, 0]
# self.goal = None
# def getResultFromClient(self):
# if (self.goal):
# return self.client.get_result()
# else:
# return None
def goToPose(self, pose):
"""Send a `NavToPose` action request."""
self.debug("Waiting for 'NavigateToPose' action server")
while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0):
self.info(
"'NavigateToPose' action server not available, waiting...")
goal_msg = NavigateToPose.Goal()
goal_msg.pose = pose
self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' +
str(pose.pose.position.y) + '...')
send_goal_future = self.nav_to_pose_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('Goal to ' + str(pose.pose.position.x) + ' ' +
str(pose.pose.position.y) + ' was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True