def goToPose(self, pose, behavior_tree=''): """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 goal_msg.behavior_tree = behavior_tree 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
def __init__(self): super().__init__('route_manager', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) self.route = [] self.current_goal = NavigateToPose.Goal() self.client = ActionClient(self, NavigateToPose, 'NavigateToPose') self.client.wait_for_server() time.sleep(10) route_file_info = self.get_parameter('route_file').value # route file info is in the form "<package-name>.<path from install's share directory>" route_pkg_share = get_package_share_directory(route_file_info.split('.')[0]) route_file_path = os.path.join(route_pkg_share, '.'.join(route_file_info.split('.')[1:])) with open(route_file_path, 'r') as f: route_file_contents = f.read() route_yaml = yaml.safe_load(route_file_contents) self.route_mode = route_yaml['mode'] if self.route_mode not in RouteManager.route_modes: self.get_logger().error( "Route mode '%s' unknown, exiting route manager" % (self.route_mode,)) return poses = route_yaml['poses'] if not poses: self.get_logger().info("Route manager initialized no goals, unable to route") self.goals = RouteManager.route_modes[self.route_mode](poses) self.get_logger().info( "Route manager initialized with %s goals in %s mode" % (len(poses), self.route_mode,))
def __init__(self, x, y, theta, timeout): super().__init__('nav_demo') #self.create_timer(.1, self.timer_callback) latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) self.create_subscription(OccupancyGrid, 'map', self.map_callback, qos_profile=latching_qos) # Create the action client and wait until it is active #self.ac = ActionClient(self, NavigateToPose, '/NavigateToPose') self.ac = ActionClient(self, NavigateToPose, '/navigate_to_pose') # Set up the goal message self.goal = NavigateToPose.Goal() self.goal.pose.header.frame_id = 'map' # SEEMS TO BE IGNORED! self.goal.pose.pose.position.x = x self.goal.pose.pose.position.y = y # We need to convert theta to a quaternion.... quaternion = transformations.quaternion_from_euler(0, 0, theta, 'rxyz') self.goal.pose.pose.orientation.x = quaternion[0] self.goal.pose.pose.orientation.y = quaternion[1] self.goal.pose.pose.orientation.z = quaternion[2] self.goal.pose.pose.orientation.w = quaternion[3] self.timeout = timeout
def send_goal(self, xpose, ypose, zpose, wpose, reset=False): goal_msg = NavigateToPose.Goal() # goal_msg.pose.header.stamp.sec = round(self.get_clock().now().nanoseconds*(10**-9)) goal_msg.pose.header.frame_id = "map" goal_msg.pose.pose.position.x = xpose goal_msg.pose.pose.position.y = ypose goal_msg.pose.pose.orientation.z = zpose goal_msg.pose.pose.orientation.w = wpose self._action_client.wait_for_server() while True: self._send_goal_future = self._action_client.send_goal_async( goal_msg) # wait until feedback comes rclpy.spin_until_future_complete(self, self._send_goal_future) goal_handle = self._send_goal_future.result() if not goal_handle.accepted: self.get_logger().info( 'Goal rejected :(' ) # it seems that this case doesn't happen return #self.get_logger().info('Goal accepted :)') self._get_result_future = goal_handle.get_result_async() # wait until result comes rclpy.spin_until_future_complete(self, self._get_result_future) try: print( "current_pose after get_future : %.4f %.4f %.4f %.4f" % (self.current_pose.position.x, self.current_pose.position.y, self.current_pose.orientation.z, self.current_pose.orientation.w)) except: pass # pose not received yet time.sleep(3) status = self._get_result_future.result().status if reset: return # reset doesn't need to succeed print("goal status :", status) if ( status == GoalStatus.STATUS_SUCCEEDED ): #nav2 stack thinks goal succeeded. CAUTION : nav2 stack is not credible. It might fail and send SUCCEED. if not self.initial_pose_received: #for catching succeed bug of nav2 stack if not self.initial_pose_received: print("initial pose not received") return False print("distance :", self.distanceFromGoal(goal_msg.pose.pose)) print("yaw :", self.yawFromGoal(goal_msg.pose.pose)) # if difference is too big, send goal again if self.distanceFromGoal( goal_msg.pose.pose) > 0.15 or self.yawFromGoal( goal_msg.pose.pose ) > 17: # threshold : 15cm, 17 degree print( "too far from goal. Turtlebot will go to reset point") return False else: # succeed break return True
def goal_conv(p): goal = NavigateToPose.Goal() goal.pose.header.frame_id = p.frame_id goal.pose.pose.position.y = p.y goal.pose.pose.position.x = p.x goal.pose.pose.orientation.z = p.th return goal
def send_goal(self): print_info(f"goal {self.goal_sent_count + 1} / {self.num_goals}") if not self.navigate_to_pose_action_client.wait_for_server( timeout_sec=5.0): self.write_event(self.get_clock().now(), 'failed_to_communicate_with_navigation_node') raise RunFailException( "navigate_to_pose action server not available") if len(self.traversal_path_poses) == 0: self.write_event(self.get_clock().now(), 'insufficient_number_of_poses_in_traversal_path') raise RunFailException( "insufficient number of poses in traversal path, can not send goal" ) goal_msg = NavigateToPose.Goal() goal_msg.pose.header.stamp = self.get_clock().now().to_msg() goal_msg.pose.header.frame_id = self.fixed_frame goal_msg.pose.pose = self.traversal_path_poses.popleft() self.current_goal = goal_msg self.navigate_to_pose_action_goal_future = self.navigate_to_pose_action_client.send_goal_async( goal_msg) self.navigate_to_pose_action_goal_future.add_done_callback( self.goal_response_callback) self.write_event(self.get_clock().now(), 'target_pose_set') self.goal_sent_count += 1
def to_move_goal(self, pose): goal = NavigateToPose.Goal() goal.pose.header.stamp = self.get_clock().now().to_msg() goal.pose.header.frame_id = "map" goal.pose.pose.position = Point(**pose['pose']['position']) goal.pose.pose.orientation = Quaternion(**pose['pose']['orientation']) return goal
async def service_callback(self, request, response, goal_handle): position = request["position"] orientation = request["orientation"] self.action_client = ActionClient(self, NavigateToPose, '/NavigateToPose', callback_group=self.cb_group) if not self.action_client.wait_for_server(timeout_sec=5.0): response.message = "Abort" return response goal_msg = NavigateToPose.Goal() goal_msg.pose.header.frame_id = "map" goal_msg.pose.pose.position.x = position[0] goal_msg.pose.pose.position.y = position[1] goal_msg.pose.pose.position.z = position[2] goal_msg.pose.pose.orientation.x = orientation[0] goal_msg.pose.pose.orientation.y = orientation[1] goal_msg.pose.pose.orientation.z = orientation[2] goal_msg.pose.pose.orientation.w = orientation[3] self.move_goalhandle = await self.action_client.send_goal_async( goal_msg) if not self.move_goalhandle.accepted: self.get_logger().info("goal rejected") response.message = "Abort" return response future_result = await self.move_goalhandle.get_result_async() response.message = "Success" return response
def runNavigateAction(self, goal_pose: Optional[Pose] = None): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateToPose' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg( "'NavigateToPose' action server not available, waiting...") self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(goal_msg) rclpy.spin_until_future_complete(self, send_goal_future) goal_handle = send_goal_future.result() if not goal_handle.accepted: self.error_msg('Goal rejected') return False self.info_msg('Goal accepted') get_result_future = goal_handle.get_result_async() self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_SUCCEEDED: self.info_msg('Goal failed with status code: {0}'.format(status)) return False self.info_msg('Goal succeeded!') return True
def runNavigateAction(self, goal_pose: Optional[Pose] = None): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateToPose' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg( "'NavigateToPose' action server not available, waiting...") if (os.getenv('GROOT_MONITORING') == 'True'): if self.grootMonitoringGetStatus(): self.error_msg('Behavior Tree must not be running already!') self.error_msg('Are you running multiple goals/bts..?') return False self.info_msg('This Error above MUST Fail and is o.k.!') self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(goal_msg) rclpy.spin_until_future_complete(self, send_goal_future) goal_handle = send_goal_future.result() if not goal_handle.accepted: self.error_msg('Goal rejected') return False self.info_msg('Goal accepted') get_result_future = goal_handle.get_result_async() future_return = True if (os.getenv('GROOT_MONITORING') == 'True'): try: if not self.grootMonitoringReloadTree(): self.error_msg( 'Failed GROOT_BT - Reload Tree from ZMQ Server') future_return = False if not self.grootMonitoringGetStatus(): self.error_msg( 'Failed GROOT_BT - Get Status from ZMQ Publisher') future_return = False except Exception as e: # noqa: B902 self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message) future_return = False self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_SUCCEEDED: self.info_msg('Goal failed with status code: {0}'.format(status)) return False if not future_return: return False self.info_msg('Goal succeeded!') return True
def __init__(self): smach.State.__init__(self, outcomes=['success','failed'], input_keys=['move_base_goals_in','from_goal','goal_order'], output_keys=['from_goal']) self.goal_msgs = NavigateToPose.Goal() self.node_ = rclpy.create_node('nav2_client') self.node_.get_logger().info('Created node') self.action_client = ActionClient (self.node_,NavigateToPose,'navigate_to_pose')
def navigateToPoseFunction(self, target_pose): self.navigate_to_pose_state_ = self.e_util.NAVIGATION_MOVING target_pose_stamped = PoseStamped() target_pose_stamped.pose = target_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = target_pose_stamped self.navigate_to_pose_client_.wait_for_server() # send_goal_async test self.send_goal_future = self.navigate_to_pose_client_.send_goal_async( goal_msg) self.send_goal_future.add_done_callback( self.navigateToPoseResponseCallback) return self.send_goal_future
def request_pose(self): goal_msg = NavigateToPose.Goal() goal_msg.pose.pose.position.x = 1.5 goal_msg.pose.pose.position.y = 0.5 goal_msg.pose.pose.position.z = 0.25 goal_msg.pose.pose.orientation.x = 0.0 goal_msg.pose.pose.orientation.y = 0.0 goal_msg.pose.pose.orientation.z = 0.0 goal_msg.pose.pose.orientation.w = 1.0 self.nav_to_pose_cli.wait_for_server() self.nav_to_pose_cli.send_goal_async(goal_msg)
def send_target(self): quaternion = self.euler_to_quaternion(0, 0, self.target.z) goal = NavigateToPose.Goal() self.goal_nbr = self.goal_nbr + 1 goal.pose.header.seq = self.goal_nbr goal.pose.header.stamp = Time() goal.pose.header.frame_id = "sendGoal" goal.pose.pose.position = Point(x=self.target.x, y=self.target.y, yaw=0) goal.pose.pose.orientation.x = quaternion[0] goal.pose.pose.orientation.y = quaternion[1] goal.pose.pose.orientation.z = quaternion[2] goal.pose.pose.orientation.w = quaternion[3] self.goal = self.client.send_goal_async(goal) self.goal.add_done_callback(self.goal_reached)
def goToPose(self, pose): # Sends a `NavToPose` action request and waits for completion 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
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 feed_conv(f): print('feedback is ', f) feed = NavigateToPose.Feedback() return feed