def test_send_goal_async_with_feedback_for_another_goal(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
# Send a goal and then publish feedback
first_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
future = ac.send_goal_async(
Fibonacci.Goal(),
feedback_callback=self.feedback_callback,
goal_uuid=first_goal_uuid)
rclpy.spin_until_future_complete(self.node, future, self.executor)
# Send another goal, but without a feedback callback
second_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
future = ac.send_goal_async(Fibonacci.Goal(),
goal_uuid=second_goal_uuid)
rclpy.spin_until_future_complete(self.node, future, self.executor)
# Publish feedback for the second goal
self.mock_action_server.publish_feedback(second_goal_uuid)
self.timed_spin(1.0)
self.assertEqual(self.feedback, None)
# Publish feedback for the first goal (with callback)
self.mock_action_server.publish_feedback(first_goal_uuid)
self.timed_spin(1.0)
self.assertNotEqual(self.feedback, None)
finally:
ac.destroy()
def test_send_goal_async_no_server(self):
ac = ActionClient(self.node, Fibonacci, 'not_fibonacci')
try:
future = ac.send_goal_async(Fibonacci.Goal())
self.timed_spin(2.0)
self.assertFalse(future.done())
finally:
ac.destroy()
class SendGoal(smach.State): #Create state SendGoal
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 execute(self, userdata):
if self.action_client.wait_for_server(20)!=1:
self.node_.get_logger().error('Action server not available after waiting.')
self.destory_node()
return 'failed'
move_base_goal = userdata.move_base_goals_in[userdata.goal_order[userdata.from_goal]].split(';')
userdata.from_goal+=1
self.goal_msgs.pose.header.frame_id='map'
self.goal_msgs.pose.header.stamp=self.node_.get_clock().now().to_msg()
self.goal_msgs.pose.pose.position.x=float(move_base_goal[0])
self.goal_msgs.pose.pose.position.y=float(move_base_goal[1])
self.goal_msgs.pose.pose.position.z= 0.0
self.goal_msgs.pose.pose.orientation.x = 0.0
self.goal_msgs.pose.pose.orientation.y = 0.0
self.goal_msgs.pose.pose.orientation.z = float(move_base_goal[2])
self.goal_msgs.pose.pose.orientation.w = float(move_base_goal[3])
goal_handle_future=self.action_client.send_goal_async(self.goal_msgs)
rclpy.spin_until_future_complete(self.node_,goal_handle_future)
goal_handle = goal_handle_future.result()
if not goal_handle.accepted:
self.node_.get_logger().info('Goal rejected :(')
self.destory_node()
return 'failed'
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(self.node_,get_result_future)
result = get_result_future.result().result
status = get_result_future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.node_.get_logger().info('Succeeded!')
elif status == GoalStatus.STATUS_ABORTED:
self.node_.get_logger().error('Goal was aborted')
self.destory_node()
return 'failed'
elif status == GoalStatus.STATUS_CANCLED:
self.node_.get_logger().error('Goal was canceled')
self.destory_node()
return 'failed'
else:
self.node_.get_logger().error('Unknown result code')
self.destory_node()
return 'failed'
if userdata.from_goal == len(userdata.goal_order):
self.destory_node()
return 'success'
def destory_node(self):
self.action_client.destroy()
self.node_.destroy_node()
def test_get_num_entities(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
num_entities = ac.get_num_entities()
self.assertEqual(num_entities.num_subscriptions, 2)
self.assertEqual(num_entities.num_guard_conditions, 0)
self.assertEqual(num_entities.num_timers, 0)
self.assertEqual(num_entities.num_clients, 3)
self.assertEqual(num_entities.num_services, 0)
ac.destroy()
def test_different_type_raises(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
with self.assertRaises(TypeError):
ac.send_goal('different goal type')
with self.assertRaises(TypeError):
ac.send_goal_async('different goal type')
finally:
ac.destroy()
def test_wait_for_server_exists(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
start = time.monotonic()
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
end = time.monotonic()
self.assertGreater(0.0, end - start - TIME_FUDGE)
self.assertLess(0.0, end - start + TIME_FUDGE)
finally:
ac.destroy()
def test_constructor_no_defaults(self):
ac = ActionClient(
self.node,
Fibonacci,
'fibonacci',
goal_service_qos_profile=rclpy.qos.QoSProfile(depth=10),
result_service_qos_profile=rclpy.qos.QoSProfile(depth=10),
cancel_service_qos_profile=rclpy.qos.QoSProfile(depth=10),
feedback_sub_qos_profile=rclpy.qos.QoSProfile(depth=10),
status_sub_qos_profile=rclpy.qos.QoSProfile(depth=10))
ac.destroy()
def test_send_goal_async(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
future = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, future, self.executor)
self.assertTrue(future.done())
goal_handle = future.result()
self.assertTrue(goal_handle.accepted)
finally:
ac.destroy()
class HelloClient(Node):
def __init__(self):
super().__init__('hello_action_test_client')
self.hello_client = ActionClient(self, Hello, 'hello_action')
def goal_responseCB(self, future): # dunno what the future handles...
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info(
'Goal rejected: {0.accepted}'.format(goal_handle))
return
self.get_logger().info(
'Goal accepted: {0.accepted}'.format(goal_handle))
self.get_result_future = goal_handle.get_result_async()
self.get_result_future.add_done_callback(self.get_resultCB)
def feedbackCB(self, feedback):
# Gets feedback data by "goal_handle.publish_feedback" function from server
self.get_logger().info('Received feedback:\n{0.status}'.format(
feedback.feedback))
def get_resultCB(self, future):
result = future.result().result
status = future.result(
).status # probably "goal_handle.succeed()" from server?
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info(
'Goal succeeded!\n\t Result: {0.status}'.format(result))
else:
self.get_logger().info(
'Goal failed with status: {}'.format(status))
def send_goal(self):
self.get_logger().info('Waiting for action server...')
self.hello_client.wait_for_server()
goal_msg = Hello.Goal()
goal_msg.hello_goal = 'Hello World!'
self.get_logger().info('Sending goal request...')
self.send_goal_future = self.hello_client.send_goal_async(
goal_msg, feedback_callback=self.feedbackCB)
self.send_goal_future.add_done_callback(self.goal_responseCB)
def destroy(self):
self.hello_client.destroy()
self.destroy_node()
class TestPipelineRun(unittest.TestCase):
@classmethod
def setUpClass(cls):
rclpy.init()
@classmethod
def tearDownClass(cls):
rclpy.shutdown()
def setUp(self):
self.node = rclpy.create_node('test_node')
self.configure_client = self.node.create_client(
ConfigurePipeline, '/triton/configure_pipeline')
while not self.configure_client.wait_for_service(timeout_sec=1.0):
self.node.get_logger().warn(
'Configure pipeline service not available, waiting again...')
self.run_client = ActionClient(self.node, RunPipeline,
'/triton/run_pipeline')
while not self.run_client.wait_for_server(timeout_sec=1.0):
self.node.get_logger().warn(
'Run pipeline service not available, waiting again...')
def tearDown(self):
self.configure_client.destroy()
self.run_client.destroy()
self.node.destroy_node()
def test_run_no_configure(self, pipeline_manager, proc_info, proc_output):
goal_msg = RunPipeline.Goal()
goal_msg.input = 0
future = self.run_client.send_goal_async(goal_msg)
future_count = 0
result = None
while rclpy.ok():
rclpy.spin_once(self.node)
if future.done():
future_count += 1
if future_count == 1:
future = future.result().get_result_async()
if future_count == 2:
result = future.result().result
break
if result.output == 1:
proc_output.assertWaitFor(
expected_output='Pipeline is not configured')
else:
self.fail('Should abort due to unconfigured pipeline')
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_action_client')
action_client = ActionClient(node, Fibonacci, 'fibonacci')
node.get_logger().info('Waiting for action server...')
action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
node.get_logger().info('Sending goal request...')
send_goal_future = action_client.send_goal_async(
goal_msg,
feedback_callback=lambda feedback: feedback_cb(node.get_logger(),
feedback))
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
if not goal_handle.accepted:
node.get_logger().info('Goal rejected :(')
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
return
node.get_logger().info('Goal accepted :)')
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
result = get_result_future.result().result
status = get_result_future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
node.get_logger().info('Goal succeeded! Result: {0}'.format(
result.sequence))
else:
node.get_logger().info(
'Goal failed with status code: {0}'.format(status))
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
def test_get_result_async(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
# Send a goal
goal_future = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, goal_future, self.executor)
self.assertTrue(goal_future.done())
goal_handle = goal_future.result()
# Get the goal result
result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(self.node, result_future, self.executor)
self.assertTrue(result_future.done())
finally:
ac.destroy()
def test_send_goal_async_with_feedback_after_goal(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
# Send a goal and then publish feedback
goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
future = ac.send_goal_async(
Fibonacci.Goal(),
feedback_callback=self.feedback_callback,
goal_uuid=goal_uuid)
rclpy.spin_until_future_complete(self.node, future, self.executor)
# Publish feedback after goal has been accepted
self.mock_action_server.publish_feedback(goal_uuid)
self.timed_spin(1.0)
self.assertNotEqual(self.feedback, None)
finally:
ac.destroy()
def test_send_cancel_async(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
# Send a goal
goal_future = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, goal_future, self.executor)
self.assertTrue(goal_future.done())
goal_handle = goal_future.result()
# Cancel the goal
cancel_future = goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(self.node, cancel_future, self.executor)
self.assertTrue(cancel_future.done())
self.assertEqual(
cancel_future.result().goals_canceling[0].goal_id,
goal_handle.goal_id)
finally:
ac.destroy()
def test_send_goal_multiple(self):
ac = ActionClient(self.node,
Fibonacci,
'fibonacci',
callback_group=ReentrantCallbackGroup())
executor = MultiThreadedExecutor(context=self.context)
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
future_0 = ac.send_goal_async(Fibonacci.Goal())
future_1 = ac.send_goal_async(Fibonacci.Goal())
future_2 = ac.send_goal_async(Fibonacci.Goal())
rclpy.spin_until_future_complete(self.node, future_0, executor)
rclpy.spin_until_future_complete(self.node, future_1, executor)
rclpy.spin_until_future_complete(self.node, future_2, executor)
self.assertTrue(future_0.done())
self.assertTrue(future_1.done())
self.assertTrue(future_2.done())
self.assertTrue(future_0.result().accepted)
self.assertTrue(future_1.result().accepted)
self.assertTrue(future_2.result().accepted)
finally:
ac.destroy()
class NavTester(Node):
def __init__(self,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ''):
super().__init__(node_name='nav2_tester', namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback,
pose_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateToPose,
'navigate_to_pose')
def info_msg(self, msg: str):
self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
def warn_msg(self, msg: str):
self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
def error_msg(self, msg: str):
self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
def setInitialPose(self):
msg = PoseWithCovarianceStamped()
msg.pose.pose = self.initial_pose
msg.header.frame_id = 'map'
self.info_msg('Publishing Initial Pose')
self.initial_pose_pub.publish(msg)
self.currentPose = self.initial_pose
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose = pose
return msg
def publishGoalPose(self, goal_pose: Optional[Pose] = None):
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
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 poseCallback(self, msg):
self.info_msg('Received amcl_pose')
self.current_pose = msg.pose.pose
self.initial_pose_received = True
def reachesGoal(self, timeout, distance):
goalReached = False
start_time = time.time()
while not goalReached:
rclpy.spin_once(self, timeout_sec=1)
if self.distanceFromGoal() < distance:
goalReached = True
self.info_msg('*** GOAL REACHED ***')
return True
elif timeout is not None:
if (time.time() - start_time) > timeout:
self.error_msg('Robot timed out reaching its goal!')
return False
def distanceFromGoal(self):
d_x = self.current_pose.position.x - self.goal_pose.position.x
d_y = self.current_pose.position.y - self.goal_pose.position.y
distance = math.sqrt(d_x * d_x + d_y * d_y)
self.info_msg('Distance from goal is: ' + str(distance))
return distance
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg('Waiting for ' + node_name + ' to become active')
node_service = node_name + '/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(node_service + ' service not available, waiting...')
req = GetState.Request() # empty request
state = 'UNKNOWN'
while (state != 'active'):
self.info_msg('Getting ' + node_name + ' state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg('Result of get_state: %s' % state)
else:
self.error_msg('Exception while calling service: %r' %
future.exception())
time.sleep(5)
def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down navigation lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down navigation lifecycle manager complete.')
except Exception as e:
self.error_msg('Service call failed %r' % (e, ))
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down localization lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down localization lifecycle manager complete')
except Exception as e:
self.error_msg('Service call failed %r' % (e, ))
class WaypointFollowerTest(Node):
def __init__(self):
super().__init__(node_name='nav2_waypoint_tester', namespace='')
self.waypoints = None
self.action_client = ActionClient(self, FollowWaypoints,
'follow_waypoints')
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
self.initial_pose_received = False
self.goal_handle = None
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback,
pose_qos)
def setInitialPose(self, pose):
self.init_pose = PoseWithCovarianceStamped()
self.init_pose.pose.pose.position.x = pose[0]
self.init_pose.pose.pose.position.y = pose[1]
self.init_pose.header.frame_id = 'map'
self.publishInitialPose()
time.sleep(5)
def poseCallback(self, msg):
self.info_msg('Received amcl_pose')
self.initial_pose_received = True
def setWaypoints(self, waypoints):
self.waypoints = []
for wp in waypoints:
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose.position.x = wp[0]
msg.pose.position.y = wp[1]
msg.pose.orientation.w = 1.0
self.waypoints.append(msg)
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 publishInitialPose(self):
self.initial_pose_pub.publish(self.init_pose)
def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
self.info_msg('Destroyed follow_waypoints action client')
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
rclpy.spin_until_future_complete(self, future)
future.result()
except Exception as e: # noqa: B902
self.error_msg('%s service call failed %r' % (
transition_service,
e,
))
self.info_msg('{} finished'.format(transition_service))
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
rclpy.spin_until_future_complete(self, future)
future.result()
except Exception as e: # noqa: B902
self.error_msg('%s service call failed %r' % (
transition_service,
e,
))
self.info_msg('{} finished'.format(transition_service))
def cancel_goal(self):
cancel_future = self.goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(self, cancel_future)
def info_msg(self, msg: str):
self.get_logger().info(msg)
def warn_msg(self, msg: str):
self.get_logger().warn(msg)
def error_msg(self, msg: str):
self.get_logger().error(msg)
class NavTester(Node):
def __init__(self,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ''):
super().__init__(node_name='nav2_tester', namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback,
pose_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateToPose,
'navigate_to_pose')
def info_msg(self, msg: str):
self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
def warn_msg(self, msg: str):
self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
def error_msg(self, msg: str):
self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
def setInitialPose(self):
msg = PoseWithCovarianceStamped()
msg.pose.pose = self.initial_pose
msg.header.frame_id = 'map'
self.info_msg('Publishing Initial Pose')
self.initial_pose_pub.publish(msg)
self.currentPose = self.initial_pose
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose = pose
return msg
def publishGoalPose(self, goal_pose: Optional[Pose] = None):
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
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 grootMonitoringReloadTree(self):
# ZeroMQ Context
context = zmq.Context()
sock = context.socket(zmq.REQ)
port = 1667 # default server port for groot monitoring
# # Set a Timeout so we do not spin till infinity
sock.setsockopt(zmq.RCVTIMEO, 1000)
# sock.setsockopt(zmq.LINGER, 0)
sock.connect('tcp://localhost:' + str(port))
self.info_msg('ZMQ Server Port: ' + str(port))
# this should fail
try:
sock.recv()
self.error_msg(
'ZMQ Reload Tree Test 1/3 - This should have failed!')
# Only works when ZMQ server receives a request first
sock.close()
return False
except zmq.error.ZMQError:
self.info_msg('ZMQ Reload Tree Test 1/3: Check')
try:
# request tree from server
sock.send_string('')
# receive tree from server as flat_buffer
sock.recv()
self.info_msg('ZMQ Reload Tree Test 2/3: Check')
except zmq.error.Again:
self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree')
sock.close()
return False
# this should fail
try:
sock.recv()
self.error_msg(
'ZMQ Reload Tree Test 3/3 - This should have failed!')
# Tree should only be loadable ONCE after ZMQ server received a request
sock.close()
return False
except zmq.error.ZMQError:
self.info_msg('ZMQ Reload Tree Test 3/3: Check')
return True
def grootMonitoringGetStatus(self):
# ZeroMQ Context
context = zmq.Context()
# Define the socket using the 'Context'
sock = context.socket(zmq.SUB)
# Set a Timeout so we do not spin till infinity
sock.setsockopt(zmq.RCVTIMEO, 2000)
# sock.setsockopt(zmq.LINGER, 0)
# Define subscription and messages with prefix to accept.
sock.setsockopt_string(zmq.SUBSCRIBE, '')
port = 1666 # default publishing port for groot monitoring
sock.connect('tcp://127.0.0.1:' + str(port))
for request in range(3):
try:
sock.recv()
except zmq.error.Again:
self.error_msg('ZMQ - Did not receive any status')
sock.close()
return False
self.info_msg('ZMQ - Did receive status')
return True
def poseCallback(self, msg):
self.info_msg('Received amcl_pose')
self.current_pose = msg.pose.pose
self.initial_pose_received = True
def reachesGoal(self, timeout, distance):
goalReached = False
start_time = time.time()
while not goalReached:
rclpy.spin_once(self, timeout_sec=1)
if self.distanceFromGoal() < distance:
goalReached = True
self.info_msg('*** GOAL REACHED ***')
return True
elif timeout is not None:
if (time.time() - start_time) > timeout:
self.error_msg('Robot timed out reaching its goal!')
return False
def distanceFromGoal(self):
d_x = self.current_pose.position.x - self.goal_pose.position.x
d_y = self.current_pose.position.y - self.goal_pose.position.y
distance = math.sqrt(d_x * d_x + d_y * d_y)
self.info_msg('Distance from goal is: ' + str(distance))
return distance
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg('Waiting for ' + node_name + ' to become active')
node_service = node_name + '/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(node_service + ' service not available, waiting...')
req = GetState.Request() # empty request
state = 'UNKNOWN'
while (state != 'active'):
self.info_msg('Getting ' + node_name + ' state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg('Result of get_state: %s' % state)
else:
self.error_msg('Exception while calling service: %r' %
future.exception())
time.sleep(5)
def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down navigation lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down navigation lifecycle manager complete.')
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e, ))
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down localization lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down localization lifecycle manager complete')
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e, ))
def wait_for_initial_pose(self):
self.initial_pose_received = False
while not self.initial_pose_received:
self.info_msg('Setting initial pose')
self.setInitialPose()
self.info_msg('Waiting for amcl_pose to be received')
rclpy.spin_once(self, timeout_sec=1)
class NavTester(Node):
def __init__(self,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ""):
super().__init__(node_name="nav2_tester", namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, "initialpose", 10)
self.goal_pub = self.create_publisher(PoseStamped, "goal_pose", 10)
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1,
)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, "amcl_pose", self.poseCallback,
pose_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateToPose,
"navigate_to_pose")
def info_msg(self, msg: str):
self.get_logger().info("\033[1;37;44m" + msg + "\033[0m")
def warn_msg(self, msg: str):
self.get_logger().warn("\033[1;37;43m" + msg + "\033[0m")
def error_msg(self, msg: str):
self.get_logger().error("\033[1;37;41m" + msg + "\033[0m")
def setInitialPose(self):
msg = PoseWithCovarianceStamped()
msg.pose.pose = self.initial_pose
msg.header.frame_id = "map"
self.info_msg("Publishing Initial Pose")
self.initial_pose_pub.publish(msg)
self.currentPose = self.initial_pose
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = "map"
msg.pose = pose
return msg
def publishGoalPose(self, goal_pose: Optional[Pose] = None):
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
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(f"Failed GROOT_BT - ZMQ Tests: {e}")
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(f"Goal failed with status code: {status}")
return False
if not future_return:
return False
self.info_msg("Goal succeeded!")
return True
def grootMonitoringReloadTree(self):
# ZeroMQ Context
context = zmq.Context()
sock = context.socket(zmq.REQ)
port = 1667 # default server port for groot monitoring
# # Set a Timeout so we do not spin till infinity
sock.setsockopt(zmq.RCVTIMEO, 1000)
# sock.setsockopt(zmq.LINGER, 0)
sock.connect(f"tcp://localhost:{port}")
self.info_msg(f"ZMQ Server Port:{port}")
# this should fail
try:
sock.recv()
self.error_msg(
"ZMQ Reload Tree Test 1/3 - This should have failed!")
# Only works when ZMQ server receives a request first
sock.close()
return False
except zmq.error.ZMQError:
self.info_msg("ZMQ Reload Tree Test 1/3: Check")
try:
# request tree from server
sock.send_string("")
# receive tree from server as flat_buffer
sock.recv()
self.info_msg("ZMQ Reload Tree Test 2/3: Check")
except zmq.error.Again:
self.info_msg("ZMQ Reload Tree Test 2/3 - Failed to load tree")
sock.close()
return False
# this should fail
try:
sock.recv()
self.error_msg(
"ZMQ Reload Tree Test 3/3 - This should have failed!")
# Tree should only be loadable ONCE after ZMQ server received a request
sock.close()
return False
except zmq.error.ZMQError:
self.info_msg("ZMQ Reload Tree Test 3/3: Check")
return True
def grootMonitoringGetStatus(self):
# ZeroMQ Context
context = zmq.Context()
# Define the socket using the 'Context'
sock = context.socket(zmq.SUB)
# Set a Timeout so we do not spin till infinity
sock.setsockopt(zmq.RCVTIMEO, 2000)
# sock.setsockopt(zmq.LINGER, 0)
# Define subscription and messages with prefix to accept.
sock.setsockopt_string(zmq.SUBSCRIBE, "")
port = 1666 # default publishing port for groot monitoring
sock.connect(f"tcp://127.0.0.1:{port}")
for request in range(3):
try:
sock.recv()
except zmq.error.Again:
self.error_msg("ZMQ - Did not receive any status")
sock.close()
return False
self.info_msg("ZMQ - Did receive status")
return True
def poseCallback(self, msg):
self.info_msg("Received amcl_pose")
self.current_pose = msg.pose.pose
self.initial_pose_received = True
def reachesGoal(self, timeout, distance):
goalReached = False
start_time = time.time()
while not goalReached:
rclpy.spin_once(self, timeout_sec=1)
if self.distanceFromGoal() < distance:
goalReached = True
self.info_msg("*** GOAL REACHED ***")
return True
elif timeout is not None:
if (time.time() - start_time) > timeout:
self.error_msg("Robot timed out reaching its goal!")
return False
def distanceFromGoal(self):
d_x = self.current_pose.position.x - self.goal_pose.position.x
d_y = self.current_pose.position.y - self.goal_pose.position.y
distance = math.sqrt(d_x * d_x + d_y * d_y)
self.info_msg(f"Distance from goal is: {distance}")
return distance
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg(f"Waiting for {node_name} to become active")
node_service = f"{node_name}/get_state"
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(f"{node_service} service not available, waiting...")
req = GetState.Request() # empty request
state = "UNKNOWN"
while state != "active":
self.info_msg(f"Getting {node_name} state...")
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg(f"Result of get_state: {state}")
else:
self.error_msg(
f"Exception while calling service: {future.exception()!r}")
time.sleep(5)
def shutdown(self):
self.info_msg("Shutting down")
self.action_client.destroy()
transition_service = "lifecycle_manager_navigation/manage_nodes"
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(
f"{transition_service} service not available, waiting...")
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg("Shutting down navigation lifecycle manager...")
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
"Shutting down navigation lifecycle manager complete.")
except Exception as e: # noqa: B902
self.error_msg(f"Service call failed {e!r}")
transition_service = "lifecycle_manager_localization/manage_nodes"
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(
f"{transition_service} service not available, waiting...")
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg("Shutting down localization lifecycle manager...")
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
"Shutting down localization lifecycle manager complete")
except Exception as e: # noqa: B902
self.error_msg(f"Service call failed {e!r}")
def wait_for_initial_pose(self):
self.initial_pose_received = False
while not self.initial_pose_received:
self.info_msg("Setting initial pose")
self.setInitialPose()
self.info_msg("Waiting for amcl_pose to be received")
rclpy.spin_once(self, timeout_sec=1)
class MissionExperimentRunNode(Node):
"""
Run a sequence of mission experiments.
The control flow is buried in the callbacks. Each experiment does 3 things:
1. sets parameters on auv_node (set_auv_node_params)
2. sets parameters on filter_node (set_filter_node_params)
3. starts a mission (send_mission_goal)
4. repeat (start_next_run_or_experiment_or_done)
"""
def __init__(self, experiments, repeat: bool = False):
super().__init__('experiment_runner')
assert len(experiments) > 0
self._experiments = experiments
# Subscribe to several messages for later processing
self._co_sub = self.create_subscription(Control, '/control', self.co_cb, 10)
self._fp_sub = self.create_subscription(FiducialPoseStamped, '/filtered_fp',
self.fp_cb, 10)
self._gt_sub = self.create_subscription(Odometry, '/ground_truth', self.gt_cb, 10)
# Set parameter service clients
self._set_param_auv_node_client = self.create_client(SetParameters,
'/auv_node/set_parameters')
self._set_param_filter_node_client = self.create_client(SetParameters,
'/filter_node/set_parameters')
# Mission action client
self._mission_action_client = ActionClient(self, Mission, '/mission')
# Futures manage the async state changes
self._set_auv_node_params_future = None
self._set_filter_node_params_future = None
self._send_goal_future = None
self._get_result_future = None
# Active mission
self._goal_handle = None
# Repeat sequence of experiments forever, or until msg_processor returns True
self._repeat = repeat
# Keep track of the current experiment
self._idx = 0
self._count = 0
# Start 1st experiment
self.start_first_experiment()
def start_first_experiment(self):
self._idx = 0
self.start_experiment()
def start_experiment(self):
"""Start next experiment."""
self.get_logger().info('starting experiment {}'.format(self._idx))
self._experiments[self._idx].log_info(self.get_logger())
self._count = 0
self.start_run()
def start_run(self):
"""Start next run."""
self.get_logger().info(
'starting run {} of {}'.format(self._count + 1, self._experiments[self._idx].count))
# Step 1
self.set_auv_node_params()
def start_next_run_or_experiment_or_done(self):
"""Start next run, or next experiment, or we're done."""
self._count += 1
if self._count < self._experiments[self._idx].count:
self.start_run()
else:
self._idx += 1
self._count = 0
if self._idx < len(self._experiments):
self.start_experiment()
elif self._repeat:
self.get_logger().info('REPEAT')
self.start_first_experiment()
else:
self.get_logger().info('DONE!')
def co_cb(self, msg: Control):
if 0 <= self._idx < len(self._experiments) and self._experiments[
self._idx].co_msgs is not None:
self._experiments[self._idx].co_msgs.append(msg)
def fp_cb(self, msg: FiducialPoseStamped):
if 0 <= self._idx < len(self._experiments) and self._experiments[
self._idx].fp_msgs is not None:
self._experiments[self._idx].fp_msgs.append(msg)
def gt_cb(self, msg: Odometry):
if 0 <= self._idx < len(self._experiments) and self._experiments[
self._idx].gt_msgs is not None:
self._experiments[self._idx].gt_msgs.append(msg)
def set_auv_node_params(self):
self.get_logger().debug('waiting for /auv_node/set_parameters server...')
self._set_param_auv_node_client.wait_for_service()
request = SetParameters.Request()
for param in self._experiments[self._idx].auv_params:
request.parameters.append(param)
self.get_logger().debug('setting auv_node params...')
self._set_auv_node_params_future = self._set_param_auv_node_client.call_async(request)
self._set_auv_node_params_future.add_done_callback(self.set_auv_node_params_done_cb)
def set_auv_node_params_done_cb(self, _):
self.get_logger().debug('auv_node params set')
# Step 2
self.set_filter_node_params()
def set_filter_node_params(self):
if len(self._experiments[self._idx].filter_params) > 0:
self.get_logger().debug('waiting for /filter_node/set_parameters server...')
self._set_param_filter_node_client.wait_for_service()
request = SetParameters.Request()
for param in self._experiments[self._idx].filter_params:
request.parameters.append(param)
self.get_logger().debug('setting filter_node params...')
self._set_filter_node_params_future = self._set_param_filter_node_client.call_async(
request)
self._set_filter_node_params_future.add_done_callback(
self.set_filter_node_params_done_cb)
else:
# Step 3
self.send_mission_goal()
def set_filter_node_params_done_cb(self, _):
self.get_logger().debug('filter_node params set')
# Step 3
self.send_mission_goal()
def send_mission_goal(self):
self.get_logger().debug('waiting for /mission server...')
self._mission_action_client.wait_for_server()
self.get_logger().debug('sending goal request...')
self._send_goal_future = self._mission_action_client.send_goal_async(
self._experiments[self._idx].get_goal_msg(),
feedback_callback=self.feedback_cb)
self._send_goal_future.add_done_callback(self.goal_response_cb)
def goal_response_cb(self, future):
goal_handle: ClientGoalHandle = future.result()
if goal_handle.accepted:
self.get_logger().info('goal accepted')
# Save client goal handle, useful if we need to abort the mission
self._goal_handle = goal_handle
# Get notified when the mission is complete
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_cb)
else:
self.get_logger().warn('goal rejected, STOPPING')
def feedback_cb(self, feedback):
self.get_logger().debug(
'feedback: {0} out of {1}'.format(feedback.feedback.targets_completed,
feedback.feedback.targets_total))
def get_result_cb(self, future):
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
result = future.result().result
self.get_logger().info(
'goal succeeded, completed {0} out of {1}'.format(result.targets_completed,
result.targets_total))
# Do any post-processing
if self._experiments[self._idx].process_messages(self.get_logger()):
self.get_logger().info('messages processor returned True, STOPPING')
else:
# Clear goal_handle
self._goal_handle = None
# Step 4
self.start_next_run_or_experiment_or_done()
else:
self.get_logger().warn('goal failed with status {0}, STOPPING'.format(status))
def stop_mission_and_destroy_client(self):
if self._goal_handle is not None:
print('stop mission')
self._goal_handle.cancel_goal_async()
self._goal_handle = None
if self._mission_action_client is not None:
print('destroy action client')
# Avoids a crash in rclpy/action/client.py
self._mission_action_client.destroy()
self._mission_action_client = None
class TestPipelineRun(unittest.TestCase):
@classmethod
def setUpClass(cls):
rclpy.init()
@classmethod
def tearDownClass(cls):
rclpy.shutdown()
def setUp(self):
self.node = rclpy.create_node('test_node')
self.configure_client = self.node.create_client(
ConfigurePipeline,
'/triton/configure_pipeline'
)
while not self.configure_client.wait_for_service(timeout_sec=1.0):
self.node.get_logger().warn('Configure pipeline service not available, waiting again...')
self.run_client = ActionClient(
self.node,
RunPipeline,
'/triton/run_pipeline'
)
while not self.run_client.wait_for_server(timeout_sec=1.0):
self.node.get_logger().warn('Run pipeline service not available, waiting again...')
def tearDown(self):
self.configure_client.destroy()
self.run_client.destroy()
self.node.destroy_node()
def test_run_comp_not_valid(self, pipeline_manager, proc_info, proc_output):
req = ConfigurePipeline.Request()
pipeline_type = PipelineType()
test_type = 'example'
pipeline_type.type = test_type
req.pipeline_type = pipeline_type
test_file_name = 'test_run_comp_not_valid'
req.config_file_name = test_file_name
future = self.configure_client.call_async(req)
while rclpy.ok():
rclpy.spin_once(self.node)
if future.done():
break
goal_msg = RunPipeline.Goal()
goal_msg.input = 0
future = self.run_client.send_goal_async(goal_msg)
future_count = 0
result = None
while rclpy.ok():
rclpy.spin_once(self.node)
if future.done():
future_count += 1
if future_count == 1:
future = future.result().get_result_async()
if future_count == 2:
result = future.result().result
break
if result.output == 1:
component = 'example::DoesNotExist'
proc_output.assertWaitFor(
expected_output='Failed to find class with the requested plugin name \'{}\' in the loaded library'.format(component))
proc_output.assertWaitFor(
expected_output='The component {} was not loaded successfully'.format(component))
proc_output.assertWaitFor(
expected_output='Aborting the pipeline')
else:
self.fail('Should abort from component not able to load')
def test_constructor_defaults(self):
# Defaults
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
ac.destroy()
class WaypointFollowerTest(Node):
def __init__(self):
super().__init__(node_name='nav2_waypoint_tester', namespace='')
self.waypoints = None
self.readyToMove = True
self.currentPose = None
self.lastWaypoint = None
self.action_client = ActionClient(self, FollowWaypoints, 'FollowWaypoints')
self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
'initialpose', 10)
self.costmapClient = self.create_client(GetCostmap, '/global_costmap/get_costmap')
while not self.costmapClient.wait_for_service(timeout_sec=1.0):
self.info_msg('service not available, waiting again...')
self.initial_pose_received = False
self.goal_handle = None
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(Odometry,
'/odom', self.poseCallback, pose_qos)
# self.costmapSub = self.create_subscription(Costmap(), '/global_costmap/costmap_raw', self.costmapCallback, pose_qos)
self.costmapSub = self.create_subscription(OccupancyGrid(), '/map', self.occupancyGridCallback, pose_qos)
self.costmap = None
self.get_logger().info('Running Waypoint Test')
def occupancyGridCallback(self, msg):
self.costmap = OccupancyGrid2d(msg)
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 costmapCallback(self, msg):
self.costmap = Costmap2d(msg)
unknowns = 0
for x in range(0, self.costmap.getSizeX()):
for y in range(0, self.costmap.getSizeY()):
if self.costmap.getCost(x, y) == 255:
unknowns = unknowns + 1
self.get_logger().info(f'Unknowns {unknowns}')
self.get_logger().info(f'Got Costmap {len(getFrontier(None, self.costmap, self.get_logger()))}')
def dumpCostmap(self):
costmapReq = GetCostmap.Request()
self.get_logger().info('Requesting Costmap')
costmap = self.costmapClient.call(costmapReq)
self.get_logger().info(f'costmap resolution {costmap.specs.resolution}')
def setInitialPose(self, pose):
self.init_pose = PoseWithCovarianceStamped()
self.init_pose.pose.pose.position.x = pose[0]
self.init_pose.pose.pose.position.y = pose[1]
self.init_pose.header.frame_id = 'map'
self.currentPose = self.init_pose.pose.pose
self.publishInitialPose()
time.sleep(5)
def poseCallback(self, msg):
self.info_msg('Received amcl_pose')
self.currentPose = msg.pose.pose
self.initial_pose_received = True
def setWaypoints(self, waypoints):
self.waypoints = []
for wp in waypoints:
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose.position.x = wp[0]
msg.pose.position.y = wp[1]
msg.pose.orientation.w = 1.0
self.waypoints.append(msg)
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("'FollowWaypoints' 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:
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 '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,))
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 publishInitialPose(self):
self.initial_pose_pub.publish(self.init_pose)
def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
self.info_msg('Destroyed FollowWaypoints action client')
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service + ' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
rclpy.spin_until_future_complete(self, future)
future.result()
except Exception as e:
self.error_msg('%s service call failed %r' % (transition_service, e,))
self.info_msg('{} finished'.format(transition_service))
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service + ' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
rclpy.spin_until_future_complete(self, future)
future.result()
except Exception as e:
self.error_msg('%s service call failed %r' % (transition_service, e,))
self.info_msg('{} finished'.format(transition_service))
def cancel_goal(self):
cancel_future = self.goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(self, cancel_future)
def info_msg(self, msg: str):
self.get_logger().info(msg)
def warn_msg(self, msg: str):
self.get_logger().warn(msg)
def error_msg(self, msg: str):
self.get_logger().error(msg)
class NavTester(Node):
def __init__(self,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ""):
super().__init__(node_name="nav2_tester", namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, "initialpose", 10)
self.goal_pub = self.create_publisher(PoseStamped, "goal_pose", 10)
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1,
)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, "amcl_pose", self.poseCallback,
pose_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateToPose,
"navigate_to_pose")
def info_msg(self, msg: str):
self.get_logger().info("\033[1;37;44m" + msg + "\033[0m")
def warn_msg(self, msg: str):
self.get_logger().warn("\033[1;37;43m" + msg + "\033[0m")
def error_msg(self, msg: str):
self.get_logger().error("\033[1;37;41m" + msg + "\033[0m")
def setInitialPose(self):
msg = PoseWithCovarianceStamped()
msg.pose.pose = self.initial_pose
msg.header.frame_id = "map"
self.info_msg("Publishing Initial Pose")
self.initial_pose_pub.publish(msg)
self.currentPose = self.initial_pose
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = "map"
msg.pose = pose
return msg
def publishGoalPose(self, goal_pose: Optional[Pose] = None):
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
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()
future_return = True
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(f"Goal failed with status code: {status}")
return False
if not future_return:
return False
self.info_msg("Goal succeeded!")
return True
def poseCallback(self, msg):
self.info_msg("Received amcl_pose")
self.current_pose = msg.pose.pose
self.initial_pose_received = True
def reachesGoal(self, timeout, distance):
goalReached = False
start_time = time.time()
while not goalReached:
rclpy.spin_once(self, timeout_sec=1)
if self.distanceFromGoal() < distance:
goalReached = True
self.info_msg("*** GOAL REACHED ***")
return True
elif timeout is not None:
if (time.time() - start_time) > timeout:
self.error_msg("Robot timed out reaching its goal!")
return False
def distanceFromGoal(self):
d_x = self.current_pose.position.x - self.goal_pose.position.x
d_y = self.current_pose.position.y - self.goal_pose.position.y
distance = math.sqrt(d_x * d_x + d_y * d_y)
self.info_msg(f"Distance from goal is: {distance}")
return distance
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg(f"Waiting for {node_name} to become active")
node_service = f"{node_name}/get_state"
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(f"{node_service} service not available, waiting...")
req = GetState.Request() # empty request
state = "UNKNOWN"
while state != "active":
self.info_msg(f"Getting {node_name} state...")
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg(f"Result of get_state: {state}")
else:
self.error_msg(
f"Exception while calling service: {future.exception()!r}")
time.sleep(5)
def shutdown(self):
self.info_msg("Shutting down")
self.action_client.destroy()
transition_service = "lifecycle_manager_navigation/manage_nodes"
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(
f"{transition_service} service not available, waiting...")
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg("Shutting down navigation lifecycle manager...")
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
"Shutting down navigation lifecycle manager complete.")
except Exception as e: # noqa: B902
self.error_msg(f"Service call failed {e!r}")
transition_service = "lifecycle_manager_localization/manage_nodes"
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(
f"{transition_service} service not available, waiting...")
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg("Shutting down localization lifecycle manager...")
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
"Shutting down localization lifecycle manager complete")
except Exception as e: # noqa: B902
self.error_msg(f"Service call failed {e!r}")
def wait_for_initial_pose(self):
self.initial_pose_received = False
while not self.initial_pose_received:
self.info_msg("Setting initial pose")
self.setInitialPose()
self.info_msg("Waiting for amcl_pose to be received")
rclpy.spin_once(self, timeout_sec=1)
class ActionClientWrapperThread(threading.Thread):
def __init__(self, node, exit_future):
threading.Thread.__init__(self)
self._node = node
self._exit_future = exit_future
self._action_client = ActionClient(node, Fibonacci, 'fibonacci')
def send_goal(self):
self._node.get_logger().info('Waiting for action server...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self._node.get_logger().info('Sending goal request...')
self._send_goal_future = self._action_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self._node.get_logger().info('Goal rejected :(')
return
self._node.get_logger().info('Goal accepted :)')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def feedback_callback(self, feedback):
self._node.get_logger().info('Received feedback: {0}'.format(feedback.feedback.sequence))
def get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self._node.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence))
else:
self._node.get_logger().info('Goal failed with status: {0}'.format(status))
# Shutdown after receiving a result
# rclpy.shutdown()
self._exit_future.set_result(None)
def destroy(self):
self._action_client.destroy()
def run(self):
print("Client is entering...")
self.send_goal()
while not self._exit_future.done():
time.sleep(1)
print("Client is running...")
print('Client is exiting...')
def main():
# A flow of typical ROS program
# 1. Initialization
rclpy.init()
# 2. Create one or more nodes
node = rclpy.create_node('commander')
fp_range = FloatingPointRange(from_value=0.2, to_value=2.0, step=0.1)
pd = ParameterDescriptor(name='parameter in commander',
type=ParameterType.PARAMETER_DOUBLE,
description='set key-in period in seconds',
read_only=False,
floating_point_range=[fp_range])
param = node.declare_parameter('key_in_period',
value=0.2,
descriptor=pd,
ignore_override=False)
# 3. Process node callbacks
publisher = node.create_publisher(String, 'keys', 10)
goal_handle = None
def goal_response_callback(future):
nonlocal goal_handle
goal_handle = future.result()
if not goal_handle.accepted:
nonlocal timer
node.get_logger().info('Goal rejected. oTL')
goal_handle = None
node.destroy_timer(timer)
timer = node.create_timer(timer_period_sec=period,
callback=unix_callback)
return
node.get_logger().info('Goal accepted!')
get_result_future = goal_handle.get_result_async()
get_result_future.add_done_callback(get_result_callback)
def get_result_callback(future):
nonlocal timer
result = future.result().result.cmds
status = future.result().status
print(status)
if status == GoalStatus.STATUS_SUCCEEDED:
msg = String()
msg.data = result[-1]
publisher.publish(msg)
node.get_logger().info('Goal succeeded! ')
elif status == GoalStatus.STATUS_CANCELED:
node.get_logger().info('Goal canceled')
else:
node.get_logger().info(
'Goal failed with status code: {}'.format(status))
node.destroy_timer(timer)
timer = node.create_timer(timer_period_sec=period,
callback=unix_callback)
def feedback_callback(fb):
node.get_logger().info('Received feedback: {}'.format(
fb.feedback.partial_cmds))
msg = String()
msg.data = fb.feedback.partial_cmds[-1]
publisher.publish(msg)
def cancel_done(future):
goals_canceling = future.result(
).goals_canceling # [action_msgs.msg.GoalInfo,...]
if len(goals_canceling) > 0:
nonlocal timer
node.get_logger().info('Goal canceled')
node.destroy_timer(timer)
timer = node.create_timer(timer_period_sec=period,
callback=unix_callback)
else:
node.get_logger().info('Goal not canceled')
def unix_callback2():
node.get_logger().info('Key \'' + autopilot_cmds['cancel_goal'] +
'\' in to cancel')
fd = sys.stdin.fileno()
old = termios.tcgetattr(fd)
tty.setcbreak(fd, tty.TCSANOW)
timeout = 0
read_ready = select.select([sys.stdin], [], [], timeout)[0]
if read_ready == [sys.stdin]:
if sys.stdin.read(1) == autopilot_cmds['cancel_goal']:
# if goal_handle == None:
# node.get_logger().info('Goal unavailable to be canceled')
# return
node.get_logger().info('Canceling goal...')
future = goal_handle.cancel_goal_async()
future.add_done_callback(cancel_done)
termios.tcsetattr(fd, termios.TCSANOW, old)
def unix_callback():
node.get_logger().info('Please key in!')
fd = sys.stdin.fileno()
old = termios.tcgetattr(fd)
tty.setcbreak(fd, tty.TCSANOW)
timeout = 0
read_ready = select.select([sys.stdin], [], [], timeout)[0]
if read_ready == [sys.stdin]:
msg = String()
msg.data = sys.stdin.read(1)
if msg.data == autopilot_cmds['request_goal']:
nonlocal timer
goal_msg = AutoPilot.Goal()
goal_msg.goal_x = 0
goal_msg.goal_y = 0
node.get_logger().info('Sending goal request...')
send_goal_future = action_client.send_goal_async(
goal_msg, feedback_callback=feedback_callback)
send_goal_future.add_done_callback(goal_response_callback)
node.destroy_timer(timer)
timer = node.create_timer(timer_period_sec=period,
callback=unix_callback2)
return
publisher.publish(msg)
termios.tcsetattr(fd, termios.TCSANOW, old)
def before_parameter(param_list: [rclpy.parameter.Parameter]):
nonlocal timer
p_list = [its for its in param_list if its.name == 'key_in_period']
period = p_list[0].get_parameter_value().double_value
node.destroy_timer(timer)
timer = node.create_timer(timer_period_sec=period,
callback=unix_callback)
return SetParametersResult(successful=True)
node.add_on_set_parameters_callback(before_parameter)
def after_parameter(msg: ParameterEvent):
if msg.node != '/commander':
return
param_msg_list = [
its for its in msg.changed_parameters
if its.name == 'key_in_period'
]
name = param_msg_list[0].name
value = param_msg_list[0].value.double_value
node.get_logger().info('parameter \'{}\' = {} sec.'.format(
name, value))
node.create_subscription(ParameterEvent, 'parameter_events',
after_parameter, 10)
action_client = ActionClient(node, AutoPilot, 'autopilot')
node.get_logger().info('Waiting for action server...')
action_client.wait_for_server()
period = param.get_parameter_value().double_value
timer = node.create_timer(timer_period_sec=period, callback=unix_callback)
node.get_logger().info('Publishing keystrokes.')
rclpy.spin(node)
action_client.destroy()
node.destroy_timer(timer)
node.destroy_node()
rclpy.shutdown()
def send_goal(action_name, action_type, goal_values, feedback_callback):
goal_handle = None
node = None
action_client = None
try:
try:
# TODO(jacobperron): This logic should come from a rosidl related package
parts = action_type.split('/')
if len(parts) == 1:
raise ValueError()
if len(parts) == 2:
parts = [parts[0], 'action', parts[1]]
package_name = parts[0]
action_type = parts[-1]
if not all(parts):
raise ValueError()
except ValueError:
raise RuntimeError('The passed action type is invalid')
module = importlib.import_module('.'.join(parts[:-1]))
action_module = getattr(module, action_type)
goal_dict = yaml.safe_load(goal_values)
rclpy.init()
node_name = NODE_NAME_PREFIX + '_send_goal_{}_{}'.format(
package_name, action_type)
node = rclpy.create_node(node_name)
action_client = ActionClient(node, action_module, action_name)
goal = action_module.Goal()
try:
set_message_fields(goal, goal_dict)
except Exception as ex:
return 'Failed to populate message fields: {!r}'.format(ex)
print('Waiting for an action server to become available...')
action_client.wait_for_server()
print('Sending goal:\n {}'.format(message_to_yaml(goal, None)))
goal_future = action_client.send_goal_async(goal, feedback_callback)
rclpy.spin_until_future_complete(node, goal_future)
goal_handle = goal_future.result()
if goal_handle is None:
raise RuntimeError('Exception while sending goal: {!r}'.format(
goal_future.exception()))
if not goal_handle.accepted:
print('Goal was rejected.')
# no need to potentially cancel the goal anymore
goal_handle = None
return
print('Goal accepted with ID: {}\n'.format(
bytes(goal_handle.goal_id.uuid).hex()))
result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, result_future)
result = result_future.result()
if result is None:
raise RuntimeError('Exception while getting result: {!r}'.format(
result_future.exception()))
# no need to potentially cancel the goal anymore
goal_handle = None
print('Result:\n {}'.format(message_to_yaml(result.result, None)))
print('Goal finished with status: {}'.format(
_goal_status_to_string(result.status)))
finally:
# Cancel the goal if it's still active
if (goal_handle is not None
and (GoalStatus.STATUS_ACCEPTED == goal_handle.status
or GoalStatus.STATUS_EXECUTING == goal_handle.status)):
print('Canceling goal...')
cancel_future = goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(node, cancel_future)
cancel_response = cancel_future.result()
if cancel_response is None:
raise RuntimeError(
'Exception while canceling goal: {!r}'.format(
cancel_future.exception()))
if len(cancel_response.goals_canceling) == 0:
raise RuntimeError('Failed to cancel goal')
if len(cancel_response.goals_canceling) > 1:
raise RuntimeError('More than one goal canceled')
if cancel_response.goals_canceling[
0].goal_id != goal_handle.goal_id:
raise RuntimeError('Canceled goal with incorrect goal ID')
print('Goal canceled.')
if action_client is not None:
action_client.destroy()
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
class NavTester(Node):
def __init__(self,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ''):
super().__init__(node_name='nav2_tester', namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
pose_qos = QoSProfile(durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback,
pose_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateThroughPoses,
'navigate_through_poses')
def info_msg(self, msg: str):
self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
def warn_msg(self, msg: str):
self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
def error_msg(self, msg: str):
self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
def setInitialPose(self):
msg = PoseWithCovarianceStamped()
msg.pose.pose = self.initial_pose
msg.header.frame_id = 'map'
self.info_msg('Publishing Initial Pose')
self.initial_pose_pub.publish(msg)
self.currentPose = self.initial_pose
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose = pose
return msg
def runNavigateAction(self, goal_pose: Optional[Pose] = None):
# Sends a `NavToPose` action request and waits for completion
self.info_msg("Waiting for 'NavigateThroughPoses' action server")
while not self.action_client.wait_for_server(timeout_sec=1.0):
self.info_msg(
"'NavigateThroughPoses' action server not available, waiting..."
)
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
goal_msg = NavigateThroughPoses.Goal()
goal_msg.poses = [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(f'Goal failed with status code: {status}')
return False
self.info_msg('Goal succeeded!')
return True
def poseCallback(self, msg):
self.info_msg('Received amcl_pose')
self.current_pose = msg.pose.pose
self.initial_pose_received = True
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg(f'Waiting for {node_name} to become active')
node_service = f'{node_name}/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(f'{node_service} service not available, waiting...')
req = GetState.Request() # empty request
state = 'UNKNOWN'
while (state != 'active'):
self.info_msg(f'Getting {node_name} state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg(f'Result of get_state: {state}')
else:
self.error_msg(
f'Exception while calling service: {future.exception()!r}')
time.sleep(5)
def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(
f'{transition_service} service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down navigation lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down navigation lifecycle manager complete.')
except Exception as e: # noqa: B902
self.error_msg(f'Service call failed {e!r}')
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(
f'{transition_service} service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down localization lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down localization lifecycle manager complete')
except Exception as e: # noqa: B902
self.error_msg(f'Service call failed {e!r}')
def wait_for_initial_pose(self):
self.initial_pose_received = False
while not self.initial_pose_received:
self.info_msg('Setting initial pose')
self.setInitialPose()
self.info_msg('Waiting for amcl_pose to be received')
rclpy.spin_once(self, timeout_sec=1)
class NavTester(Node):
def __init__(self,
test_type: TestType,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ''):
super().__init__(node_name='nav2_tester', namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
transient_local_qos = QoSProfile(
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
volatile_qos = QoSProfile(
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback,
transient_local_qos)
self.test_type = test_type
self.filter_test_result = True
if self.test_type == TestType.KEEPOUT:
self.plan_sub = self.create_subscription(Path, 'plan',
self.planCallback,
volatile_qos)
elif self.test_type == TestType.SPEED:
self.speed_it = 0
# Expected chain of speed limits
self.limits = [50.0, 0.0]
# Permissive array: all received speed limits must match to "limits" from above
self.limit_passed = [False, False]
self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit',
self.speedLimitCallback,
volatile_qos)
self.mask_received = False
self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask',
self.maskCallback,
transient_local_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateToPose,
'navigate_to_pose')
def info_msg(self, msg: str):
self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
def warn_msg(self, msg: str):
self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
def error_msg(self, msg: str):
self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
def setInitialPose(self):
msg = PoseWithCovarianceStamped()
msg.pose.pose = self.initial_pose
msg.header.frame_id = 'map'
self.info_msg('Publishing Initial Pose')
self.initial_pose_pub.publish(msg)
self.currentPose = self.initial_pose
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose = pose
return msg
def publishGoalPose(self, goal_pose: Optional[Pose] = None):
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
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 isInKeepout(self, x, y):
mx, my = self.filter_mask.worldToMap(x, y)
if mx == -1 and my == -1: # Out of mask's area
return False
if self.filter_mask.getValue(mx, my) == 100: # Occupied
return True
return False
# Checks that (x, y) position does not belong to a keepout zone.
def checkKeepout(self, x, y):
if not self.mask_received:
self.warn_msg('Filter mask was not received')
elif self.isInKeepout(x, y):
self.filter_test_result = False
self.error_msg('Pose (' + str(x) + ', ' + str(y) +
') belongs to keepout zone')
return False
return True
# Checks that currently received speed_limit is equal to the it-th item
# of expected speed "limits" array.
# If so, sets it-th item of permissive array "limit_passed" to be true.
# Otherwise it will be remained to be false.
# Also verifies that speed limit messages received no more than N-times
# (where N - is the length of "limits" array),
# otherwise sets overall "filter_test_result" to be false.
def checkSpeed(self, it, speed_limit):
if it >= len(self.limits):
self.error_msg('Got excess speed limit')
self.filter_test_result = False
return
if speed_limit == self.limits[it]:
self.limit_passed[it] = True
else:
self.error_msg('Incorrect speed limit received: ' +
str(speed_limit) + ', but should be: ' +
str(self.limits[it]))
def poseCallback(self, msg):
self.info_msg('Received amcl_pose')
self.current_pose = msg.pose.pose
self.initial_pose_received = True
if self.test_type == TestType.KEEPOUT:
if not self.checkKeepout(msg.pose.pose.position.x,
msg.pose.pose.position.y):
self.error_msg('Robot goes into keepout zone')
def planCallback(self, msg):
self.info_msg('Received plan')
for pose in msg.poses:
if not self.checkKeepout(pose.pose.position.x,
pose.pose.position.y):
self.error_msg('Path plan intersects with keepout zone')
return
def speedLimitCallback(self, msg):
self.info_msg('Received speed limit: ' + str(msg.speed_limit))
self.checkSpeed(self.speed_it, msg.speed_limit)
self.speed_it += 1
def maskCallback(self, msg):
self.info_msg('Received filter mask')
self.filter_mask = FilterMask(msg)
self.mask_received = True
def wait_for_filter_mask(self, timeout):
start_time = time.time()
while not self.mask_received:
self.info_msg('Waiting for filter mask to be received ...')
rclpy.spin_once(self, timeout_sec=1)
if (time.time() - start_time) > timeout:
self.error_msg('Time out to waiting filter mask')
return False
return True
def reachesGoal(self, timeout, distance):
goalReached = False
start_time = time.time()
while not goalReached:
rclpy.spin_once(self, timeout_sec=1)
if self.distanceFromGoal() < distance:
goalReached = True
self.info_msg('*** GOAL REACHED ***')
return True
elif timeout is not None:
if (time.time() - start_time) > timeout:
self.error_msg('Robot timed out reaching its goal!')
return False
def distanceFromGoal(self):
d_x = self.current_pose.position.x - self.goal_pose.position.x
d_y = self.current_pose.position.y - self.goal_pose.position.y
distance = math.sqrt(d_x * d_x + d_y * d_y)
self.info_msg('Distance from goal is: ' + str(distance))
return distance
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg('Waiting for ' + node_name + ' to become active')
node_service = node_name + '/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(node_service + ' service not available, waiting...')
req = GetState.Request() # empty request
state = 'UNKNOWN'
while (state != 'active'):
self.info_msg('Getting ' + node_name + ' state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg('Result of get_state: %s' % state)
else:
self.error_msg('Exception while calling service: %r' %
future.exception())
time.sleep(5)
def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down navigation lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down navigation lifecycle manager complete.')
except Exception as e:
self.error_msg('Service call failed %r' % (e, ))
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down localization lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg(
'Shutting down localization lifecycle manager complete')
except Exception as e:
self.error_msg('Service call failed %r' % (e, ))
def wait_for_initial_pose(self):
self.initial_pose_received = False
while not self.initial_pose_received:
self.info_msg('Setting initial pose')
self.setInitialPose()
self.info_msg('Waiting for amcl_pose to be received')
rclpy.spin_once(self, timeout_sec=1)
def send_goal(action_name, action_type, goal_values, feedback_callback):
goal_handle = None
node = None
action_client = None
try:
try:
action_module = get_action(action_type)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError('The passed action type is invalid')
goal_dict = yaml.safe_load(goal_values)
rclpy.init()
node_name = f"{NODE_NAME_PREFIX}_send_goal_{action_type.replace('/', '_')}"
node = rclpy.create_node(node_name)
action_client = ActionClient(node, action_module, action_name)
goal = action_module.Goal()
try:
set_message_fields(goal, goal_dict)
except Exception as ex:
return 'Failed to populate message fields: {!r}'.format(ex)
print('Waiting for an action server to become available...')
action_client.wait_for_server()
print('Sending goal:\n {}'.format(message_to_yaml(goal)))
goal_future = action_client.send_goal_async(goal, feedback_callback)
rclpy.spin_until_future_complete(node, goal_future)
goal_handle = goal_future.result()
if goal_handle is None:
raise RuntimeError(
'Exception while sending goal: {!r}'.format(goal_future.exception()))
if not goal_handle.accepted:
print('Goal was rejected.')
# no need to potentially cancel the goal anymore
goal_handle = None
return
print('Goal accepted with ID: {}\n'.format(bytes(goal_handle.goal_id.uuid).hex()))
result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, result_future)
result = result_future.result()
if result is None:
raise RuntimeError(
'Exception while getting result: {!r}'.format(result_future.exception()))
# no need to potentially cancel the goal anymore
goal_handle = None
print('Result:\n {}'.format(message_to_yaml(result.result)))
print('Goal finished with status: {}'.format(_goal_status_to_string(result.status)))
finally:
# Cancel the goal if it's still active
if (goal_handle is not None and
(GoalStatus.STATUS_ACCEPTED == goal_handle.status or
GoalStatus.STATUS_EXECUTING == goal_handle.status)):
print('Canceling goal...')
cancel_future = goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(node, cancel_future)
cancel_response = cancel_future.result()
if cancel_response is None:
raise RuntimeError(
'Exception while canceling goal: {!r}'.format(cancel_future.exception()))
if len(cancel_response.goals_canceling) == 0:
raise RuntimeError('Failed to cancel goal')
if len(cancel_response.goals_canceling) > 1:
raise RuntimeError('More than one goal canceled')
if cancel_response.goals_canceling[0].goal_id != goal_handle.goal_id:
raise RuntimeError('Canceled goal with incorrect goal ID')
print('Goal canceled.')
if action_client is not None:
action_client.destroy()
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
