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)