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
