class MinimalActionClient(Node):
def __init__(self):
super().__init__('minimal_action_client')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
def cancel_done(self, future):
cancel_response = future.result()
if len(cancel_response.goals_canceling) > 0:
self.get_logger().info('Goal successfully canceled')
else:
self.get_logger().info('Goal failed to cancel')
rclpy.shutdown()
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self._goal_handle = goal_handle
self.get_logger().info('Goal accepted :)')
# Start a 2 second timer
self._timer = self.create_timer(2.0, self.timer_callback)
def feedback_callback(self, feedback):
self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.sequence))
def timer_callback(self):
self.get_logger().info('Canceling goal')
# Cancel the goal
future = self._goal_handle.cancel_goal_async()
future.add_done_callback(self.cancel_done)
# Cancel the timer
self._timer.cancel()
def send_goal(self):
self.get_logger().info('Waiting for action server...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self.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 send_goals(node, action_type, tests):
import rclpy
from rclpy.action import ActionClient
action_client = ActionClient(node, action_type, 'test/action/' + action_type.__name__)
if not action_client.wait_for_server(20):
print('Action server not available after waiting', file=sys.stderr)
return 1
test_index = 0
while rclpy.ok() and test_index < len(tests):
print('Sending goal for test number {}'.format(test_index), file=sys.stderr)
test = tests[test_index]
invalid_feedback = False
# On feedback, check the feedback is valid
def feedback_callback(feedback):
nonlocal invalid_feedback
if not test.is_feedback_valid(feedback):
invalid_feedback = True
goal_handle_future = action_client.send_goal_async(
test.goal,
feedback_callback=feedback_callback)
rclpy.spin_until_future_complete(node, goal_handle_future)
goal_handle = goal_handle_future.result()
if not goal_handle.accepted:
print('Goal rejected', file=sys.stderr)
return 1
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
result = get_result_future.result()
if not test.is_result_valid(result):
return 1
if invalid_feedback:
return 1
test_index += 1
return 0
class MinimalActionClient(Node):
def __init__(self):
super().__init__('minimal_action_client')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.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.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.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence))
else:
self.get_logger().info('Goal failed with status: {0}'.format(status))
# Shutdown after receiving a result
rclpy.shutdown()
def send_goal(self):
self.get_logger().info('Waiting for action server...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self.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 main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_action_client')
leg_name = 'front_left'
forcesensor_sub = node.create_subscription(ForceSensor, 'hapthexa/leg/'+leg_name+'/force_sensor', lambda msg: forcesensor_callback(msg, node),10)
action_client = ActionClient(node, MoveLeg, 'hapthexa/leg/'+leg_name+'/move_leg')
node.get_logger().info('Waiting for action server...')
action_client.wait_for_server()
global goal_handle
global phase
phase = 1
global once_failed
once_failed = False
goal_msg = MoveLeg.Goal()
goal_msg.x = float(0)
goal_msg.y = float(0)
goal_msg.z = float(10.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
phase = 2
goal_msg = MoveLeg.Goal()
goal_msg.x = float(5.0)
goal_msg.y = float(0)
goal_msg.z = float(10.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
if once_failed:
phase = 3
goal_msg = MoveLeg.Goal()
goal_msg.x = float(0.0)
goal_msg.y = float(0)
goal_msg.z = float(15.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
phase = 4
goal_msg = MoveLeg.Goal()
goal_msg.x = float(5.0)
goal_msg.y = float(0)
goal_msg.z = float(15.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
phase = 5
goal_msg = MoveLeg.Goal()
goal_msg.x = float(5.0)
goal_msg.y = float(0)
goal_msg.z = float(0.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
class NavigationActionClient(Node):
def __init__(self):
DRONE_DEVICE_ID = os.getenv('DRONE_DEVICE_ID')
super().__init__("navigation_action_client", namespace = DRONE_DEVICE_ID)
self._action_done_event = Event()
self._action_cancel_event = Event()
self._action_response_result = False
self._action_cancel_result = False
self._goal_handle = None
self._action_callback_group = MutuallyExclusiveCallbackGroup()
self._action_client = ActionClient(self, NavigateToPose, "/" + DRONE_DEVICE_ID + "/navigate_to_pose", callback_group = self._action_callback_group)
self._service_callback_group = MutuallyExclusiveCallbackGroup()
self._local_waypoint_service = self.create_service(Vec4, '~/local_waypoint', self.local_waypoint_callback, callback_group = self._service_callback_group)
self._gps_waypoint_service = self.create_service(Vec4, '~/gps_waypoint', self.gps_waypoint_callback, callback_group = self._service_callback_group)
self._cancel_goal = self.create_service(Trigger, '~/cancel_goal', self.cancel_goal_callback, callback_group = self._service_callback_group)
def local_waypoint_callback(self, request, response):
self.get_logger().info('Incoming local waypoint request: \t{0}'.format(request.goal))
self.send_goal(request.goal, True)
# Wait for action to be done
self._action_done_event.wait()
response.success = self._action_response_result
if response.success:
response.message = "Goal accepted"
else:
response.message = "Goal rejected"
return response
def gps_waypoint_callback(self, request, response):
self.get_logger().info('Incoming gps waypoint request: \t{0}'.format(request.goal))
self.send_goal(request.goal, False)
# Wait for action to be done
self._action_done_event.wait()
response.success = self._action_response_result
if response.success:
response.message = "Goal accepted"
else:
response.message = "Goal rejected"
return response
def cancel_goal_callback(self, request, response):
self.get_logger().info('Incoming request to cancel goal')
if self._goal_handle is None:
response.success = False
response.message = "No active goal"
return response
self.cancel_goal()
# Wait for action to be done
self._action_cancel_event.wait()
response.success = self._action_cancel_result
if response.success:
response.message = "Goal canceled"
else:
response.message = "Goal failed to cancel"
return response
def send_goal(self, goal, is_local):
self.get_logger().info('Waiting for action server')
self._action_client.wait_for_server()
pose = PoseStamped()
pose.header.stamp = rclpy.clock.Clock().now().to_msg()
if is_local:
pose.header.frame_id = "world"
else:
pose.header.frame_id = "geographic_world"
point = Point()
point.x = float(goal[0])
point.y = float(goal[1])
point.z = float(goal[2])
pose.pose.position = point
q = quaternion_from_euler(0,0,goal[3])
pose.pose.orientation = q
goal_msg = NavigateToPose.Goal()
goal_msg.pose = pose
goal_msg.not_auto_land = True
self.get_logger().info('Sending goal request...')
self._action_done_event.clear()
self._action_response_result = False
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)
return
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('Goal rejected :(')
# Signal that action is done
self._action_done_event.set()
return
self.get_logger().info('Goal accepted :)')
self._action_response_result = True
# Signal that action is done
self._action_done_event.set()
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
self._goal_handle = goal_handle
def get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded!')
elif status == GoalStatus.STATUS_ABORTED:
self.get_logger().error('Goal aborted!')
elif status == GoalStatus.STATUS_CANCELED:
self.get_logger().error('Goal canceled!')
self._goal_handle = None
def feedback_callback(self, feedback_msg):
self.get_logger().info('Received feedback: {0}'.format(feedback_msg))
def cancel_goal(self):
self.get_logger().info('Canceling goal')
self._action_cancel_event.clear()
self._action_cancel_result = False
# Cancel the goal
future = self._goal_handle.cancel_goal_async()
future.add_done_callback(self.cancel_done)
def cancel_done(self, future):
cancel_response = future.result()
if len(cancel_response.goals_canceling) > 0:
self.get_logger().info('Goal successfully canceled')
self._action_cancel_result = True
self._goal_handle = None
else:
self.get_logger().error('Goal failed to cancel')
self._action_cancel_event.set()
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:
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:
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)
print('Lost')
else:
print('Unknown state', state)
print(str(result))
def active_cb():
print("Server accepted action")
def feedback_cb(feedback):
if len(sys.argv) > 1 and '--feedback' in sys.argv:
print('Feedback')
print(feedback)
print()
print('[..] Connecting to action server \'dynup\'', end='')
sys.stdout.flush()
client = ActionClient(node, Dynup, 'dynup')
if not client.wait_for_server():
exit(1)
print('\r[OK] Connecting to action server \'dynup\'')
print()
goal = Dynup.Goal()
goal.direction = sys.argv[1]
client.send_goal_async(goal)
client.done_cb = done_cb
client.feedback_cb = feedback_cb
client.active_cb = active_cb
print("Sent new goal")
def __init__(self, name, actionName):
super().__init__(name)
self.client = ActionClient(self, FollowJointTrajectory, actionName)
self.remainingIteration = 0
self.currentTrajectory = None
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()
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_ABORTED:
self.info_msg(f'Goal failed with status code: {status}')
return False
self.info_msg('Goal failed, as expected!')
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)
def __init__(self):
super().__init__('turtle_action_client')
self.turtle_action = ActionClient(self, SquareSpiral, 'Spiral64')
class followJointTrajectoryClient(Node):
def __init__(self, name, actionName):
super().__init__(name)
self.client = ActionClient(self, FollowJointTrajectory, actionName)
self.remainingIteration = 0
self.currentTrajectory = None
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected.')
return
self.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.get_logger().info('Received feedback.')
def get_result_callback(self, future):
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded.')
else:
self.get_logger().info(
'Goal failed with status: {0}'.format(status))
if self.remainingIteration > 0:
self.send_goal(self.currentTrajectory, self.remainingIteration - 1)
else:
# Shutdown after receiving a result
rclpy.shutdown()
def send_goal(self, trajectory, iteration=1):
self.get_logger().info('Waiting for action server...')
self.client.wait_for_server()
self.currentTrajectory = trajectory
self.remainingIteration = iteration - 1
goal_msg = FollowJointTrajectory.Goal()
goal_msg.trajectory.joint_names = trajectory['joint_names']
for point in trajectory['points']:
duration = Duration(
sec=int(point['time_from_start']['sec']),
nanosec=int(int(point['time_from_start']['nanosec']) * 1.0e+6))
trajectoryPoint = JointTrajectoryPoint(
positions=point['positions'],
velocities=point['velocities'],
accelerations=point['accelerations'],
time_from_start=duration)
goal_msg.trajectory.points.append(trajectoryPoint)
self.get_logger().info('Sending goal request...')
self._send_goal_future = self.client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
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 __init__(self):
super().__init__('minimal_action_client')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
def __init__(self):
super().__init__('fibonacci_action_client')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
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.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1)
volatile_qos = QoSProfile(
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
reliability=QoSReliabilityPolicy.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: # 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)
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()
class MinimalActionClient():
def __init__(self, action_type, node, server_name):
self._server_name = server_name
if action_type == "Fibonacci":
self._action_client = ActionClient(node, Fibonacci, server_name)
self.__result = None
self.__state = "Free"
def get_result(self):
return self.__result
def set_result(self, val):
self.__result = val
def get_state(self):
return self.__state
def set_state(self, new_state):
self.__state = new_state
def listener_callback(self, msg):
self.get_logger().info('I heard: "%s"' % msg.data)
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
print('Goal rejected :( ' + self._server_name)
return
print('Goal accepted :) ' + self._server_name)
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):
print('Received feedback: {0}'.format(feedback.feedback.sequence))
def get_result_callback(self, future):
result = future.result().result
if self._server_name == "fibonacci_1":
self.__result = 't1'
elif self._server_name == "fibonacci_2":
self.__result = None
elif self._server_name == "fibonacci_3":
self.__result = None
elif self._server_name == "fibonacci_4":
self.__result = 't3'
elif self._server_name == "fibonacci_5":
self.__result = 't3'
elif self._server_name == "fibonacci_6":
self.__result = 't3'
elif self._server_name == "fibonacci_7":
self.__result = 't4'
elif self._server_name == "fibonacci_8":
self.__result = 't4'
elif self._server_name == "fibonacci_9":
self.__result = None
elif self._server_name == "fibonacci_10":
self.__result = 't6'
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
print(self._server_name +
': Goal succeeded! Result: {0}'.format(result.sequence))
self.__state = "Done"
else:
print(self._server_name +
': Goal failed with status: {0}'.format(status))
# Shutdown after receiving a result
# rclpy.shutdown()
def send_goal(self, order):
# self.get_logger().info('Waiting for action server '+self._server_name)
print('Waiting for action server ' + self._server_name)
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = order
# self.get_logger().info('Sending goal request to '+self._server_name)
print('Sending goal request to ' + self._server_name)
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 __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.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1)
volatile_qos = QoSProfile(
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
reliability=QoSReliabilityPolicy.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.clearing_ep_sub = self.create_subscription(
PointCloud2, 'local_costmap/clearing_endpoints',
self.clearingEndpointsCallback, transient_local_qos)
self.test_type = test_type
self.filter_test_result = True
self.clearing_endpoints_received = False
self.voxel_marked_received = False
self.voxel_unknown_received = False
self.cost_cloud_received = False
if self.test_type == TestType.KEEPOUT:
self.plan_sub = self.create_subscription(Path, 'plan',
self.planCallback,
volatile_qos)
self.voxel_marked_sub = self.create_subscription(
PointCloud2, 'voxel_marked_cloud', self.voxelMarkedCallback, 1)
self.voxel_unknown_sub = self.create_subscription(
PointCloud2, 'voxel_unknown_cloud', self.voxelUnknownCallback,
1)
self.cost_cloud_sub = self.create_subscription(
PointCloud2, 'cost_cloud', self.dwbCostCloudCallback, 1)
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 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)))
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()
class DroneTester(Node):
def __init__(
self,
goal_pose: [0, 0, 0, 0],
tolerance_position: [0.5],
timeout: [60],
):
super().__init__(node_name='drone_tester')
self.target_system = 1
self.goal_pose_array = goal_pose
self.timeout_array = timeout
self.tolerance_position_array = tolerance_position
self.number_of_goals = len(self.goal_pose_array)
self.index_of_goals = 0
self.goal_pose = self.goal_pose_array[self.index_of_goals]
# print("pose to reach: ", self.goal_pose)
self.set_flight_mode_action_client = ActionClient(self, SetFlightMode, 'set_flight_mode')
self.action_response_received = False
self.action_response_success = False
self.action_response_result = None
self.vehicle_command_pub = self.create_publisher(PoseStamped,
self.get_namespace() +
'/command_pose',
1)
self.model_pose_sub = self.create_subscription(NavSatFix,
self.get_namespace() +
'/gps',
self.poseCallback, 10)
self.flight_mode_sub = self.create_subscription(FlightMode,
self.get_namespace() +
'/flight_mode',
self.flightModeCallback, 10)
self.initial_pose_received = False
self.current_pose = None
self.altitude_ref = 0
self.landed = -1
def send_goal(self, mode):
self.get_logger().info('Waiting for action server...')
self.set_flight_mode_action_client.wait_for_server()
goal_msg = SetFlightMode.Goal()
goal_msg.goal.flight_mode = mode
self.get_logger().info('Sending goal request...')
self._send_goal_future = self.set_flight_mode_action_client.send_goal_async(
goal_msg)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def get_result_callback(self, future):
result = future.result().result
status = future.result().status
self.action_response_received = True
self.action_response_success = result.success
self.action_response_result = result.result.flight_mode
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded!')
else:
self.get_logger().info('Goal failed')
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.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 arm_vehicle(self):
self.info_msg("arming vehicle")
while(self.current_pose == None):
rclpy.spin_once(self, timeout_sec=0.1)
self.send_goal(FlightMode.FLIGHT_MODE_ARMED)
self.action_response_received = False
while(not self.action_response_received):
rclpy.spin_once(self, timeout_sec=0.1)
self.info_msg("arming vehicle response received")
return (self.action_response_result == FlightMode.FLIGHT_MODE_ARMED)
def takeoff_vehicle(self):
self.info_msg("takeoff vehicle")
while(self.current_pose == None):
rclpy.spin_once(self, timeout_sec=0.1)
self.altitude_ref = self.current_pose.altitude
self.send_goal(FlightMode.FLIGHT_MODE_FLYING)
self.action_response_received = False
while(not self.action_response_received):
rclpy.spin_once(self, timeout_sec=0.1)
self.info_msg("takeoff vehicle response received")
return (self.action_response_result == FlightMode.FLIGHT_MODE_FLYING)
def RTL_vehicle(self):
self.goal_pose[0] = 0
self.goal_pose[1] = 0
self.goal_pose[2] = 0
self.info_msg("RTL vehicle")
self.send_goal(FlightMode.FLIGHT_MODE_RTL)
self.action_response_received = False
while(not self.action_response_received):
rclpy.spin_once(self, timeout_sec=0.1)
self.info_msg("RTL vehicle response received")
return (self.action_response_result == FlightMode.FLIGHT_MODE_RTL)
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 euler_to_quaternion(self, yaw, pitch, roll):
qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
return [qx, qy, qz, qw]
def setGoalPose(self, index_of_goal):
msg = PoseStamped();
msg.header.stamp = self.get_clock().now().to_msg();
self.goal_pose = self.goal_pose_array[index_of_goal]
self.info_msg(
'Starting tester, robot going from ' + self.goal_pose[0] + ', ' + self.goal_pose[1] +
', ' + self.goal_pose[2] + ', ' + self.goal_pose[3] + 'º. Timeout: '
+ str(self.timeout_array[index_of_goal][0]) + ", Tolerance: " + str(self.tolerance_position_array[index_of_goal][0]) )
msg.pose.position.x = float(self.goal_pose[1])
msg.pose.position.y = float(self.goal_pose[0])
msg.pose.position.z = float(self.goal_pose[2])
q = self.euler_to_quaternion(-float(self.goal_pose[3])+1.57, 0, 0)
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1];
msg.pose.orientation.z = q[2];
msg.pose.orientation.w = q[3];
self.vehicle_command_pub.publish(msg)
def poseCallback(self, msg):
self.current_pose = msg
if(not self.initial_pose_received):
self.info_msg('Received vehicle_gps_position')
self.reference_pose = (msg.latitude, msg.longitude, msg.altitude)
print(self.reference_pose)
self.initial_pose_received = True
def flightModeCallback(self, msg):
self.landed = msg.flight_mode == FlightMode.FLIGHT_MODE_DISARMED
def reachesGoal(self, timeout, distance):
goalReached = False
start_time = time.time()
while not goalReached:
rclpy.spin_once(self, timeout_sec=1)
if(not self.initial_pose_received):
continue
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 reachesTakeOffAltitude(self, timeout, distance):
altitudeReached = False
start_time = time.time()
while not altitudeReached:
rclpy.spin_once(self, timeout_sec=1)
if self.distanceTakeOff() > 2.5:
altitudeReached = True
self.info_msg('*** TakeOff altitude REACHED ***')
return True
elif timeout is not None:
if (time.time() - start_time) > timeout:
self.error_msg('Robot timed out reaching its takeoff altitude!')
return False
def reachesRTL(self, timeout, distance):
homeReached = False
start_time = time.time()
while not homeReached:
rclpy.spin_once(self, timeout_sec=1)
if self.landed and self.distanceFromGoal() < distance:
homeReached = True
self.info_msg('*** RTL REACHED ***')
return True
elif timeout is not None:
if (time.time() - start_time) > timeout:
self.error_msg('Robot timed out reaching its RTL!')
return False
def distanceFromGoal(self):
geodetic_coord = (self.current_pose.latitude,
self.current_pose.longitude,
self.current_pose.altitude)
x, y, z = pm.geodetic2ned(*geodetic_coord, *self.reference_pose)
d_x = float(self.goal_pose[0]) - x
d_y = float(self.goal_pose[1]) - y
d_z = float(self.goal_pose[2]) + z
distance = math.sqrt(d_x * d_x + d_y * d_y + d_z * d_z)
self.info_msg('Distance from goal is: ' + str(distance))
return distance
def distanceTakeOff(self):
return self.current_pose.altitude - self.altitude_ref
