def net_swarm_behavior_data(message, bridge):
behavior_msg = ap_msg.BehaviorParameters()
behavior_msg.id = message.data_type
behavior_msg.params = message.data
bridge.publish('recv_swarm_data',
behavior_msg,
latched=True,
queue_size=50)
def net_swarm_behavior(message, bridge):
# Process differently based on ordered type (behavior, pause, suspend)
if type(message) == messages.SuspendSwarmBehavior:
behavior_msg = ap_msg.BehaviorParameters()
behavior_msg.id = enums.SWARM_STANDBY
behavior_msg.params = []
bridge.callService('run_behavior', ap_srv.SetBehavior,\
params=behavior_msg)
# This one will go away once all of the behavior-specific
# messages are fully incorporated into SwarmCommander
elif type(message) == messages.SwarmBehavior:
behavior_msg = ap_msg.BehaviorParameters()
behavior_msg.id = message.swarm_behavior
behavior_msg.params = message.swarm_parameters
bridge.callService('run_behavior', ap_srv.SetBehavior,\
params=behavior_msg)
elif type(message) == messages.PauseSwarmBehavior:
bridge.callService('pause_behavior',
ap_srv.SetBoolean,
enable=message.behavior_pause)
def _make_firing_reports(self):
''' Publishes ROS messages associated with firing
A FiringReportStamped is published to the firing_reports topic for
ROS bagging (and any other) purposes and a SwarmBehaviorData message
is published to the send_swarm_behavior_data topic for network relay
'''
(roll, pitch, yaw) = \
quat.quat_to_euler((self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w))
report_msg = apmsg.FiringReportStamped()
report_msg.header.stamp = self._shot_time
report_msg.report.lat = self._own_pose.pose.pose.position.lat
report_msg.report.lon = self._own_pose.pose.pose.position.lon
report_msg.report.alt = self._own_pose.pose.pose.position.alt
report_msg.report.yaw = yaw
report_msg.report.target_id = self._target_id
report_msg.report.target_lat = self._target_lat
report_msg.report.target_lon = self._target_lon
report_msg.report.target_alt = self._target_alt
self._firing_report_publisher.publish(report_msg)
self._firing_report_number += 1
parser = bytes.FiringReportParser()
parser.report_num = self._firing_report_number
parser.time_secs = self._shot_time.secs
parser.time_nsecs = self._shot_time.nsecs
parser.lat = self._own_pose.pose.pose.position.lat
parser.lon = self._own_pose.pose.pose.position.lon
parser.alt = self._own_pose.pose.pose.position.alt
parser.heading = yaw
parser.target_id = self._target_id
parser.target_lat = self._target_lat
parser.target_lon = self._target_lon
parser.target_alt = self._target_alt
data = parser.pack()
net_msg = apmsg.BehaviorParameters()
net_msg.id = bytes.FIRING_REPORT
net_msg.params = data
for rep in range(0, 4): # Hope to get at least one through
self._behavior_data_publisher.publish(net_msg)
self.log_info("Greedy shooter fired at UAV %d: (%f, %f, %f)" \
%(self._target_id, self._target_lat, self._target_lon, \
self._target_alt))
def set_active(self, activate):
''' Override of the Behavior method
Not usually a good idea to override this method, but this just collects
and logs data upon deactivation and calls the parent class method
@param activate: Boolean value to activate or deactivate the behavior
@return active status upon method completion
'''
if not activate and self.is_active:
self.log_info("%s summary: Done=%s, Rounds=%d, Messages=%d, Bytes=%d" \
%(self.nodeName, str(self._done), self._sorter.rounds, \
self._sorter.xmit_msgs, self._sorter.xmit_bytes))
parser = bytes.ConsensusSummaryParser()
parser.algorithm = self._algorithm
parser.rounds = self._sorter.rounds
parser.messages = self._sorter.xmit_msgs
parser.bytes = self._sorter.xmit_bytes
msg = apmsg.BehaviorParameters()
msg.id = bytes.CONSENSUS_SUMMARY
msg.params = parser.pack()
self._behaviorDataPublisher.publish(msg)
return super(AltitudeSort, self).set_active(activate)
def pursuit_status_update(self):
if self.overtime:
return
parser = pursueBytes.PursuitMessageParser()
parser.friendly_id = self._id
if self._target_id != None:
parser.target_id = self._target_id
parser.target_distance = self.target_dist
else:
parser.target_id = 0
parser.target_distance = self._max_range
parser.pursuit_status = self.pursuit_status
data = parser.pack()
net_msg = apmsg.BehaviorParameters()
net_msg.id = pursueBytes.PURSUIT_MESSAGE
net_msg.params = data
for _ in range(1): # Hope to get at least one through
self.pubs['network/send_swarm_behavior_data'].publish(net_msg)