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()
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()