def __init__(self, mav_id, mav_num):
self.arm_state = False
self.offboard_state = False
self.state = None
self.command = TwistStamped()
self.start_point = PoseStamped()
self.feb_pos = PoseStamped()
self.feb_bias_pos = Point32()
self.obs = Point32()
self.obs_vel = Point32()
self.drone_id = mav_id
self.drone_num = mav_num
self.is_attack = False
self.state_drone = UInt64()
self.attack_num = UInt64()
self.attack_pos = [195, 10, 1.5]
mav_x = [0, -30, 0]
mav_y = [60, 30, 0]
self.start_point.pose.position.x = mav_x[self.drone_id - 1]
self.start_point.pose.position.y = mav_y[self.drone_id - 1]
self.start_point.pose.position.z = 4
'''
ros publishers
'''
self.vel_pub = rospy.Publisher(
'/drone_%s/mavros/setpoint_velocity/cmd_vel' % (self.drone_id),
TwistStamped,
queue_size=10)
self.drone_state_pub = rospy.Publisher("/drone_%s/state" %
(self.drone_id),
UInt64,
queue_size=10)
'''
ros subscriber
'''
# self.local_pos_sub = rospy.Subscriber('/drone_%s/mavros/setpoint_position/local'%(self.drone_id), PoseStamped, self.local_pos_cb)
self.bias_pos_sub = rospy.Subscriber(
'/drone_%s/mavros/local_position/pose_cor' % (self.drone_id),
PoseStamped, self.bias_cb)
self.obs_sub = rospy.Subscriber("ue4_ros/drone_3/pos", Point32,
self.obj_cb)
self.state_sub = rospy.Subscriber("/drone_%s/state" % (self.drone_id),
UInt64, self.state_cb)
self.attack_key = rospy.Subscriber("/is_attrack", UInt64,
self.attack_cb)
'''
ros services
'''
self.armService = rospy.ServiceProxy(
'/drone_%s/mavros/cmd/arming' % (self.drone_id), CommandBool)
self.flightModeService = rospy.ServiceProxy(
'/drone_%s/mavros/set_mode' % (self.drone_id), SetMode)
self.takeoffService = rospy.ServiceProxy(
'/drone_%s/mavros/cmd/takeoff' % (self.drone_id), CommandTOL)
print("obj Controller Initialized!")
def test_dictionary_with_uint64(self):
from std_msgs.msg import UInt64
expected_message = UInt64(data = 0xFFFFFFFFFFFFFFFF)
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt64', dictionary)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def test_ros_message_with_uint64(self):
from std_msgs.msg import UInt64
expected_dictionary = { 'data': 0xFFFFFFFFFFFFFFFF }
message = UInt64(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def start(self):
global drone_state, drone_state_pub
cnt_pipe = 0
while not rospy.is_shutdown():
pipes = self.play["Drone1"]
len_pipe = len(pipes)
if drone_state == 20:
a = Pipeline()
a.pipetype.data = pipes[cnt_pipe]["pipetype"]
for u in pipes[cnt_pipe]["units"]:
b = Pipeunit()
b.middle = Point32(u[0][0], u[0][1], u[0][2])
b.left = Point32(u[1][0], u[1][1], u[1][2])
b.right = Point32(u[2][0], u[2][1], u[2][2])
if len(u) > 3:
b.bottom = Point32(u[3][0], u[3][1], u[3][2])
b.up = Point32(u[4][0], u[4][1], u[4][2])
a.units.append(b)
print(a)
cnt_pipe += 1
drone_state = 30
drone_state_pub.publish(UInt64(drone_state))
self.pipe_pub.publish(a)
if cnt_pipe >= len_pipe: break
self.rate.sleep()
def callback(data):
dipswitch_value1 = 0
dipswitch_value2 = 0
dipswitch_value3 = 0
dipswitch_value4 = 0
for pin in data.pins:
if pin.pinNumber == DIPSWITCH_PIN1:
dipswitch_value1 = int(pin.pinValue)
elif pin.pinNumber == DIPSWITCH_PIN2:
dipswitch_value2 = int(pin.pinValue)
elif pin.pinNumber == DIPSWITCH_PIN3:
dipswitch_value3 = int(pin.pinValue)
elif pin.pinNumber == DIPSWITCH_PIN4:
dipswitch_value4 = int(pin.pinValue)
# Dipswitch have pin 1 at left, so it will be MSB
userid = 0
userid += dipswitch_value1 << 3
userid += dipswitch_value2 << 2
userid += dipswitch_value3 << 1
userid += dipswitch_value4 << 0
msg = UInt64()
msg = userid
pub.publish(userid)
def idle(self):
print("I'm in idle state!")
global drone_state, drone_state_pub
idle_cmd = TwistStamped()
while not rospy.is_shutdown():
if drone_state == 40:
self.vel_pub.publish(idle_cmd)
drone_state_pub.publish(UInt64(40))
self.rate.sleep()
def __init__(self):
self.arm_state = False
self.offboard_state = False
self.state = None
self.mav_pos = PoseStamped()
self.start_sphere = Point32()
self.mav_feb = Point32()
self.obj_pos = Point32()
self.attack_num = UInt64()
self.sphere_msg = Obj()
self.people_msg = Obj()
self.sphere_msg.id = 50
self.sphere_msg.type = 152
self.sphere_msg.size.x = 0.1
self.sphere_msg.size.y = 0.1
self.sphere_msg.size.z = 0.1
self.sphere_msg.position.x = 195 #0
self.sphere_msg.position.y = 10 #30
self.sphere_msg.position.z = -0.1 #0
self.people_msg.id = 60
self.people_msg.type = 30
self.people_msg.position.x = 195 #0
self.people_msg.position.y = 10 #30
self.people_msg.position.z = 0 #0
self.people_msg.angule.z = 0
self.people_msg.size.x = 1
self.people_msg.size.y = 1
self.people_msg.size.z = 1
self.start_sphere.x = self.people_msg.position.x
self.start_sphere.y = self.people_msg.position.y
self.start_sphere.z = self.people_msg.position.z
'''
ros publishers
'''
self.obj_pub = rospy.Publisher("ue4_ros/obj", Obj, queue_size=10)
self.obj_pos_pub = rospy.Publisher("ue4_ros/drone_3/pos",
Point32,
queue_size=10)
self.attack_key = rospy.Subscriber("/is_attrack", UInt64,
self.attack_cb)
'''
ros subscriber
'''
# self.local_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.local_pos_cb)
self.bias_pos_sub = rospy.Subscriber(
'/drone_1/mavros/local_position/pose_cor', PoseStamped,
self.bias_cb)
print("obj Controller Initialized!")
def start(self):
rate = rospy.Rate(20)
for _ in range(10):
self.vel_pub.publish(self.command)
self.arm_state = self.arm()
self.offboard_state = self.offboard()
rate.sleep()
desire = [0, 0, 0]
while (rospy.is_shutdown() is False):
# target = np.array([self.start_point.pose.position.x, self.start_point.pose.position.y, \
# self.start_point.pose.position.z])
feb = np.array([self.feb_bias_pos.x, self.feb_bias_pos.y, \
self.feb_bias_pos.z])
# if self.is_attack == True:
# if self.state_drone_1 == 40 and self.state_drone_2 == 40 and self.state_drone_3 == 40:
# self.is_attack == True
if self.attack_num.data == 20:
self.is_attack = True
if self.is_attack == True:
drone_state = 50
self.drone_state_pub.publish(UInt64(drone_state))
target = np.array([
self.attack_pos[0], self.attack_pos[1], self.attack_pos[2]
])
desire = self.pos_control(target, feb, 0.8, 5)
else:
desire[0] = 0
desire[1] = 0
desire[2] = 0
self.obs_vel.x = 0
self.obs_vel.y = 0
self.obs_vel.z = 0
direct_obs = -1 #(self.feb_bias_pos.x - self.obs.x)/abs(self.feb_bias_pos.x - self.obs.x)
if abs(self.feb_bias_pos.x -
self.obs.x) < 8 and self.drone_id == 1:
self.obs_vel.y = direct_obs * 2
self.command.twist.linear.x = desire[0] + self.obs_vel.x
self.command.twist.linear.y = desire[1] + self.obs_vel.y
self.command.twist.linear.z = desire[2] + self.obs_vel.z
self.command.twist.angular.z = 0
if self.state_drone.data == 50:
self.vel_pub.publish(self.command)
rate.sleep()
rospy.spin()
def service_handler(self, input):
print('service call received')
response = InfoGainsResponse()
ros_image = input.multi_channel_occupancy_grid
np_arr = np.fromstring(ros_image.data,
np.uint8).reshape(ros_image.height,
ros_image.width, 3)
print('image size', np_arr.shape)
cv2.imwrite('/tmp/costmap_srv.png', np_arr)
frontiers_data = OneDataInfoBase(
np_arr,
self._convert_frontier_info({
'BoundingBoxes': input.frontier_rois,
'Frontiers': input.frontier_clusters
}))
response.info_gains = []
batch_input = []
batch_info = []
for i in range(len(frontiers_data)):
image, info = frontiers_data[i]
image = torch.from_numpy(image)
batch_input.append(image)
batch_info.append(info)
batch_input = torch.stack(batch_input)
batch_input = Variable(batch_input)
if self.args.cuda:
batch_input = batch_input.cuda()
output, _, _ = self.model(batch_input)
info_gain = compute_expected_information_gain(batch_input, output,
batch_info,
'info_gain_srv')
for i in range(len(info_gain)):
msg = UInt64(info_gain[i]['information_gain'])
response.info_gains.append(msg)
return response
def calc_eta(self, event):
""" Calculates the Estimated Time of Arrival to the destination
"""
# Attempt an update only if the cart is driving
if self.current_state.is_navigating:
# Where are we at and how much further must we go
current_node = self.get_closest_point(self.global_pose.position.x,
self.global_pose.position.y)
distance_remaining = self.calc_trip_dist(self.local_points,
current_node)
# Remaining time in seconds
remaining_time = distance_remaining / self.cur_speed
eta_msg = UInt64()
# Calculate the ETA to the end
arrival_time = time.time() + remaining_time
# Convert the time to milliseconds
eta_msg.data = int(arrival_time * (1000))
self.eta_pub.publish(eta_msg)
def string2uint64(self, in_string):
ans = UInt64(uuid.UUID(in_string).int)
return ans
def __init__(self):
self.TSData = UInt64()
self.button_input = [0 for i in range(11)]
self.axes_input = [0 for i in range(8)]
rospy.Subscriber('/joy', Joy, self.Joystick_CallBack)
self.TSPub = rospy.Publisher('/ThrusterStates', UInt64, queue_size = 10)
drone_state_pub.publish(UInt64(drone_state))
self.pipe_pub.publish(a)
if cnt_pipe >= len_pipe: break
self.rate.sleep()
def drone_state_cb(msg):
global drone_state
drone_state = msg.data
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('-p', '--play', default=os.path.join(os.path.expanduser('~'),"Swarm_ws/src/decision/scenarios","play.json"), help='play file path')
args = parser.parse_args()
print(args)
play_file = open(args.play)
play = json.load(play_file)
print(json.dumps(play))
rospy.init_node('decision_node', anonymous=True)
drone_state_sub = rospy.Subscriber('drone_1/state', UInt64, drone_state_cb)
drone_state_pub = rospy.Publisher('drone_1/state', UInt64, queue_size=10)
drone_state = 10
drone_state_pub.publish(UInt64(drone_state))
px4 = Px4Controller()
px4.start()
drone_state = 20
drone_state_pub.publish(UInt64(drone_state))
dd = Decision(play)
dd.start()
print("Finish")
def __init__(self):
self.imu = None
self.gps = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.local_enu_position = None
self.arm_state = False
self.offboard_state = False
self.received_imu = False
self.frame = "BODY"
self.imu_rate = np.array([0, 0, 0])
self.state = None
self.mavros_state = State()
self.command = TwistStamped()
self.drone_state = UInt64()
# self.drone_state = String()
self.mav_pos = np.array([0, 0, 0])
self.mav_vel = np.array([0, 0, 0])
self.cmd_vel = np.array([0, 0, 0])
self.mav_yaw = 0
self.cmd_yaw = 0
self.mav_roll = 0
self.mav_pitch = 0
self.mav_R = np.zeros((3, 3))
#第一个任务起始点位置
self.initial_pos = np.array([147, -8 - 5, 4])
#搜索点位置
self.search_pos = np.array([[150, 3, 4], [180, 4.5, 4], [244, 1.2, 4],
[244, 20, 4], [160, 18, 4], [170, 11, 4]])
self.search_point = 0
self.target_pos = np.array([200, 10, 4])
self.dis_initial_pos = 2
self.dis_search_pos = 1
self.dis_target_pos = 1
self.P_pos = 0.8
self.P_yaw = 0.5
self.pos_vel_sat = 5
self.yaw_sat = 0.8
'''
ros subscribers /balance_force /ttc_mean
'''
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
self.local_pose_callback)
self.mavros_sub = rospy.Subscriber("/mavros/state", State,
self.mavros_state_callback)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global",
NavSatFix, self.gps_callback)
self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu,
self.imu_callback)
self.mav_sub = rospy.Subscriber("mavros/local_position/velocity_local",
TwistStamped, self.mav_vel_cb)
self.drone_state_sub = rospy.Subscriber('drone_1/state', UInt64,
self.drone_state_cb)
# self.imu_sub = rospy.Subscriber("mavros/imu/data", Imu, self.mav_vel_cb)
self.is_offboard = False
self.is_start, self.is_tunnel, self.is_trapezoid, self.is_search = False, False, False, False
'''
ros publishers
'''
self.vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
# self.drone_state_pub = rospy.Publisher('drone_1/state', String, queue_size=10)
self.drone_state_pub = rospy.Publisher('drone_1/state',
UInt64,
queue_size=10)
self.task_takeoff = 10
self.task_out_search = 20
self.task_recognize_door = 30
self.task_pass_through = 40
self.task_in_search = 50
self.task_recognize_target = 60
self.task_attack = 70
self.task_finish = 80
'''
caes1:"task_takeoff"
caes2:"task_out_search"
caes3:"task_recognize_door"
caes4:"task_pass_through"
caes5:"task_in_search"
caes6:"task_recognize_target"
caes7:"task_attack"
caes8:"task_finish"
'''
'''
ros services
'''
self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.flightModeService = rospy.ServiceProxy('/mavros/set_mode',
SetMode)
print("Px4 Controller Initialized!")
state_1 = msg
def drone2_state_cb(msg):
global state_2
state_2 = msg
def drone3_state_cb(msg):
global state_3
state_3 = msg
if __name__ == '__main__':
rospy.init_node('is_attack_node', anonymous=True)
state_1 = UInt64()
state_2 = UInt64()
state_3 = UInt64()
attack = UInt64()
drone1_state_sub = rospy.Subscriber("/drone_1/state", UInt64,
drone1_state_cb)
drone2_state_sub = rospy.Subscriber("/drone_2/state", UInt64,
drone2_state_cb)
drone3_state_sub = rospy.Subscriber("/drone_3/state", UInt64,
drone3_state_cb)
drone_state_pub = rospy.Publisher("/is_attrack", UInt64, queue_size=10)
rate = rospy.Rate(20)
while (rospy.is_shutdown() is False):
if state_1.data == 40 and state_2.data == 40 and state_3.data == 40:
attack.data = 20
continue
try:
resp = ServerProxy(node_api).getPid('/NODEINFO')
except:
rospy.logerr("[cpu monitor] failed to get pid of node %s (api is %s)" % (node, node_api))
else:
node_map[node] = Node(node, resp[2])
rospy.loginfo("[cpu monitor] adding new node %s" % node)
for node_name, node in list(node_map.items()):
if node.alive():
node.publish()
else:
rospy.logwarn("[cpu monitor] lost node %s" % node_name)
del node_map[node_name]
cpu_publish.publish(Float32(psutil.cpu_percent()))
cpu_id = [4,6,9,11]
for idx,pt in enumerate(temp_publishers):
id = "tsens_tz_sensor" + str(cpu_id[idx])
pt.publish(Float32(psutil.sensors_temperatures()[id][0].current * 100))
vm = psutil.virtual_memory()
for mem_topic, mem_publisher in zip(mem_topics, mem_publishers):
mem_publisher.publish(UInt64(getattr(vm, mem_topic)))
rospy.sleep(POLL_PERIOD)
def publish(self): self.cpu_publisher.publish(Float32(self.proc.cpu_percent())) self.mem_publisher.publish(UInt64(self.proc.memory_info().rss))
def pub_time(self):
msg = UInt64()
msg.data = self.time_elapsed
self.p_time.publish(msg)
