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 __init__(self, mav_id, mav_num):
self.arm_state = False
self.offboard_state = False
self.state = None
self.mav_pos = PoseStamped()
self.mav_bias = Point32()
self.start_sphere = Point32()
self.drone_id = mav_id
self.drone_num = mav_num
self.sphere_msg = Obj()
self.people_msg = Obj()
self.sphere_msg.id = 60 + self.drone_id
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 = 0
self.sphere_msg.position.y = 30
self.sphere_msg.position.z = 0
self.people_msg.id = 50
self.people_msg.type = 30
self.people_msg.position.x = 0
self.people_msg.position.y = 30
self.people_msg.position.z = 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_%s/pos" % (param_id),
Point32,
queue_size=10)
'''
ros subscriber
'''
# self.local_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.local_pos_cb)
self.bias_pos_sub = rospy.Subscriber(
'/drone_%s/mavros/local_position/pose_cor' % (param_id), Point32,
self.bias_cb)
print("obj Controller Initialized!")
def sphere_control():
global sphere_pos_x, sphere_pos_y, sphere_pos_z
obj_msg = Obj()
obj_msg.id = 100
obj_msg.type = 152
obj_msg.position.x = sphere_pos_x
obj_msg.position.y = sphere_pos_y
obj_msg.position.z = sphere_pos_z
obj_msg.size.x = 0.05
obj_msg.size.y = 0.05
obj_msg.size.z = 0.05
sphere_pub.publish(obj_msg)
def draw_unit(id, start, end):
global obj_pub
obj_msg = Obj()
obj_msg.id = id
obj_msg.type = 27
obj_msg.position.x = (start.x + end.x) / 2
obj_msg.position.y = (start.y + end.y) / 2
obj_msg.position.z = start.z
obj_msg.angule.z = np.arctan2(end.y - start.y, end.x - start.x)
obj_msg.size.x = np.linalg.norm([end.y - start.y, end.x - start.x])
obj_msg.size.y = 0.1
obj_msg.size.z = 2
obj_pub.publish(obj_msg)
print(obj_msg)
def send_control():
rospy.init_node('obj_send', anonymous=True)
obj_pub = rospy.Publisher("ue4_ros/obj", Obj, queue_size=10)
interval_rate = 50
interval_time = 1.0 / interval_rate
rate = rospy.Rate(interval_rate)
obj_msg = Obj()
obj_msg.id = 100
obj_msg.type = 152
obj_msg.position.x = 0
obj_msg.position.y = 3
obj_msg.position.z = 2
obj_msg.angule.x = 0
obj_msg.angule.y = 0
obj_msg.angule.z = 0
obj_msg.size.x = 1
obj_msg.size.y = 1
obj_msg.size.z = 1
while not rospy.is_shutdown():
obj_msg.position.y = obj_msg.position.y + 0.001
obj_pub.publish(obj_msg)
rate.sleep()
print("obj_z is ok {}".format(obj_z))
print("obj_pitch is ok {}".format(obj_pitch))
print("obj_yaw is ok {}".format(obj_yaw))
# last_left = left
# last_right = right
last_bottom = bottom
last_up = up
last_front = front
last_height = height
last_weight = weight
if __name__ == '__main__':
rospy.init_node('pipeline_vision', anonymous=True)
global obj_pub, obj_msg
obj_msg = Obj()
obj_pub = rospy.Publisher("ue4_ros/obj", Obj, queue_size=10)
rate = rospy.Rate(1)
rospy.Subscriber("decision_info", Pipeline, pipeline_cb)
# while not rospy.is_shutdown():
# obj_msg.id = 2
# obj_msg.type = 24#30
# obj_msg.position.x = 0
# obj_msg.position.y = 0
# obj_msg.position.z = 0
# obj_msg.angule.x = 0
# obj_msg.angule.y = 0
# obj_msg.angule.z = 0#0.25*math.pi#-30/2
# obj_msg.size.x = 1
# obj_msg.size.y = 1
# obj_msg.size.z = 1
def start(self):
rospy.init_node("offboard_node")
rate = rospy.Rate(20)
obj_pub = rospy.Publisher(
"ue4_ros/obj", Obj, queue_size=10)
# type_pub = rospy.Publisher(
# "ue4_ros/obj/vehicle_type", UInt64, queue_size=10)
# id_pub = rospy.Publisher(
# "ue4_ros/obj/vehicle_id", UInt64, queue_size=10)
obj_msg = Obj()
# type_msg = UInt64()
# id_msg = UInt64()
while(not self.mavros_state.connected):
print(self.mavros_state.connected)
rate.sleep()
for i in range(10):
self.vel_pub.publish(self.command)
self.drone_state.data == self.task_takeoff
self.drone_state_pub.publish(self.drone_state)
rate.sleep()
self.offboard_state = SetMode()
self.offboard_state.custom_mode = "OFFBOARD"
self.arm_state = CommandBool()
self.arm_state.value = True
start_time = rospy.Time.now()
'''
main ROS thread
'''
self.drone_state.data == self.task_takeoff
cnt = -1
obj_msg.position.x = 0
obj_msg.position.y = 3
obj_msg.position.z = 2
obj_msg.angule.x = 0
obj_msg.angule.y = 0
obj_msg.angule.z = 0
obj_msg.size.x = 5
obj_msg.size.y = 5
obj_msg.size.z = 1
obj_msg.type = 24
obj_msg.id = 100
while (rospy.is_shutdown() is False):
cnt += 1
# self.record_stast()
# self.drone_state_pub.publish(self.drone_state)
print("self.mav_yaw: {}".format(self.mav_yaw))
self.state_pub()
if self.is_offboard == False:
if self.mavros_state.mode == "OFFBOARD":
resp1 = self.flightModeService(0, "POSCTL")
if cnt % 10 == 0:
print("Enter MANUAL mode")
rate.sleep()
# start_yaw = self.mav_yaw
continue
else:
if self.mavros_state.mode != "OFFBOARD":
resp1 = self.flightModeService(
0, self.offboard_state.custom_mode)
if resp1.mode_sent:
print("Offboard enabled")
start_time = rospy.Time.now()
# cmd_control = self.state_control(self.drone_state.data)
# self.cmd_vel = np.array(
# [cmd_control[0], cmd_control[1], cmd_control[2]])
# self.cmd_yaw = cmd_control[3]
self.cmd_vel = np.array([0, 0, 0])
des_pos = np.array([10, 0, 3])
self.cmd_yaw = 0.1
if self.is_start == True:
self.cmd_vel = self.pos_control(
des_pos, self.mav_pos, self.P_pos, 1)
# self.cmd_vel = np.array([0, 0, 0])
# self.cmd_vel[0] = 1
self.cmd_yaw = 0.1
obj_msg.position.x = obj_msg.position.x + 0.05
# obj_msg.angule.x = obj_msg.angule.x + 0.05
# if (self.drone_state.data == self.task_takeoff or self.drone_state.data == self.task_in_search or
# self.drone_state.data == self.task_recognize_target or self.drone_state.data == self.task_attack or
# self.drone_state.data == self.task_finish or self.drone_state.data == 0):
# self.command.twist.linear.x = self.cmd_vel[0]
# self.command.twist.linear.y = self.cmd_vel[1]
# self.command.twist.linear.z = self.cmd_vel[2]
# self.command.twist.angular.z = self.cmd_yaw
# self.vel_pub.publish(self.command)
# print("mav_pos: {}".format(self.mav_pos))
self.command.twist.linear.x = self.cmd_vel[0]
self.command.twist.linear.y = self.cmd_vel[1]
self.command.twist.linear.z = self.cmd_vel[2]
self.vel_pub.publish(self.command)
obj_pub.publish(obj_msg)
# type_pub.publish(type_msg)
# id_pub.publish(id_msg)
rate.sleep()