diff --git a/demo/tutorial_7/multi_uavs_motion_planning/commander_uav1.py b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav1.py new file mode 100644 index 00000000..e72a47be --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav1.py @@ -0,0 +1,105 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav1_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav1/gi/set_pose/position', PoseStamped, queue_size=10) + self.velocity_target_pub = rospy.Publisher('/uav1/gi/set_pose/velocity', Vector3Stamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav1/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav1/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav1/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + self.velocity_target_pub.publish(self.set_speed(vx, vy, vz, BODY_OFFSET_ENU)) + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_OFFSET_ENU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + def set_speed(self, vx=0, vy=0, vz=0,BODY_OFFSET_ENU = True): + velocity = Vector3Stamped() + velocity.header.stamp=rospy.Time.now() + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + velocity.header.frame_id = 'base_link' + + else: + velocity.header.frame_id = 'map' + + velocity.vector.x = vx + velocity.vector.y = vy + velocity.vector.z = vz + + return velocity + + +if __name__ == "__main__": + + con = Commander() + time.sleep(10) + i=1 + j=0 + theta_=0.0 + theta = 0.0 + while True: + theta_=theta + theta =math.atan2(50*math.sin(2*math.pi/50*i)-con.local_pose.pose.position.y,50*math.cos(2*math.pi/50*i)-con.local_pose.pose.position.x) + if theta<0: + theta=theta+2*math.pi + con.move(vx=math.pi*math.cos(theta),vy=math.pi*math.sin(theta)) + time.sleep(0.02) + j=j+1 + if j==100: + i=i+1 + j=0 + con.land() + + diff --git a/demo/tutorial_7/multi_uavs_motion_planning/commander_uav2.py b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav2.py new file mode 100644 index 00000000..ee9dbaa1 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav2.py @@ -0,0 +1,105 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav2_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav2/gi/set_pose/position', PoseStamped, queue_size=10) + self.velocity_target_pub = rospy.Publisher('/uav2/gi/set_pose/velocity', Vector3Stamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav2/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav2/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + self.velocity_target_pub.publish(self.set_speed(vx, vy, vz, BODY_OFFSET_ENU)) + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_OFFSET_ENU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + def set_speed(self, vx=0, vy=0, vz=0,BODY_OFFSET_ENU = True): + velocity = Vector3Stamped() + velocity.header.stamp=rospy.Time.now() + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + velocity.header.frame_id = 'base_link' + + else: + velocity.header.frame_id = 'map' + + velocity.vector.x = vx + velocity.vector.y = vy + velocity.vector.z = vz + + return velocity + + +if __name__ == "__main__": + + con = Commander() + time.sleep(10) + i=1 + j=0 + theta_=0.0 + theta = 0.0 + while True: + theta_=theta + theta =math.atan2(50*math.sin(2*math.pi/50*i)-con.local_pose.pose.position.y,50*math.cos(2*math.pi/50*i)-con.local_pose.pose.position.x) + if theta<0: + theta=theta+2*math.pi + con.move(vx=(math.pi+1)*math.cos(theta),vy=(math.pi+1)*math.sin(theta)) + time.sleep(0.02) + j=j+1 + if j==100: + i=i+1 + j=0 + con.land() + + diff --git a/demo/tutorial_7/multi_uavs_motion_planning/commander_uav3.py b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav3.py new file mode 100644 index 00000000..0c6b41e8 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav3.py @@ -0,0 +1,105 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav3_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav3/gi/set_pose/position', PoseStamped, queue_size=10) + self.velocity_target_pub = rospy.Publisher('/uav3/gi/set_pose/velocity', Vector3Stamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav3/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav3/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav3/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + self.velocity_target_pub.publish(self.set_speed(vx, vy, vz, BODY_OFFSET_ENU)) + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_OFFSET_ENU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + def set_speed(self, vx=0, vy=0, vz=0,BODY_OFFSET_ENU = True): + velocity = Vector3Stamped() + velocity.header.stamp=rospy.Time.now() + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + velocity.header.frame_id = 'base_link' + + else: + velocity.header.frame_id = 'map' + + velocity.vector.x = vx + velocity.vector.y = vy + velocity.vector.z = vz + + return velocity + + +if __name__ == "__main__": + + con = Commander() + time.sleep(10) + i=1 + j=0 + theta_=0.0 + theta = 0.0 + while True: + theta_=theta + theta =math.atan2(50*math.sin(2*math.pi/50*i)-con.local_pose.pose.position.y,50*math.cos(2*math.pi/50*i)-con.local_pose.pose.position.x) + if theta<0: + theta=theta+2*math.pi + con.move(vx=(math.pi-1)*math.cos(theta),vy=(math.pi-1)*math.sin(theta)) + time.sleep(0.02) + j=j+1 + if j==100: + i=i+1 + j=0 + con.land() + + diff --git a/demo/tutorial_7/multi_uavs_motion_planning/commander_uav4.py b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav4.py new file mode 100644 index 00000000..0f00d1ce --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/commander_uav4.py @@ -0,0 +1,105 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav4_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav4/gi/set_pose/position', PoseStamped, queue_size=10) + self.velocity_target_pub = rospy.Publisher('/uav4/gi/set_pose/velocity', Vector3Stamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav4/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav4/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav4/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + self.velocity_target_pub.publish(self.set_speed(vx, vy, vz, BODY_OFFSET_ENU)) + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_OFFSET_ENU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + def set_speed(self, vx=0, vy=0, vz=0,BODY_OFFSET_ENU = True): + velocity = Vector3Stamped() + velocity.header.stamp=rospy.Time.now() + # ROS uses ENU internally, so we will stick to this convention + + if BODY_OFFSET_ENU: + velocity.header.frame_id = 'base_link' + + else: + velocity.header.frame_id = 'map' + + velocity.vector.x = vx + velocity.vector.y = vy + velocity.vector.z = vz + + return velocity + + +if __name__ == "__main__": + + con = Commander() + time.sleep(10) + i=1 + j=0 + theta_=0.0 + theta = 0.0 + while True: + theta_=theta + theta =math.atan2(50*math.sin(2*math.pi/50*i)-con.local_pose.pose.position.y,50*math.cos(2*math.pi/50*i)-con.local_pose.pose.position.x) + if theta<0: + theta=theta+2*math.pi + con.move(vx=(math.pi-2)*math.cos(theta),vy=(math.pi-2)*math.sin(theta)) + time.sleep(0.02) + j=j+1 + if j==100: + i=i+1 + j=0 + con.land() + + diff --git a/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav1.py b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav1.py new file mode 100644 index 00000000..ad4a9c59 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav1.py @@ -0,0 +1,369 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 20 + self.local_enu_position = None + + self.cur_target_pose = PoseStamped() + self.cur_target_twist=TwistStamped() + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + self.flag = 0 + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav1/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav1/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav1/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav1/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav1/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_velocity_sub = rospy.Subscriber("/uav1/gi/set_pose/velocity", Vector3Stamped, self.set_target_velocity_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav1/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav1/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.pose_target_pub = rospy.Publisher('/uav1/mavros/setpoint_position/local', PoseStamped, queue_size=10) + self.twist_target_pub = rospy.Publisher('/uav1/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav1/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav1/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav1_offboard_node") + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, y=self.local_pose.pose.position.y, z=self.takeoff_height) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.pose_target_pub.publish(self.cur_target_pose) + self.twist_target_pub.publish(self.cur_target_twist) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + if(self.flag==0): + self.pose_target_pub.publish(self.cur_target_pose) + else: + self.twist_target_pub.publish(self.cur_target_twist) + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_pose_target(self, x=0, y=0, z=0): + target_raw_pose = PoseStamped() + target_raw_pose.header.stamp = rospy.Time.now() + target_raw_pose.pose.position.y = x + target_raw_pose.pose.position.y = y + target_raw_pose.pose.position.z = z + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + + def body2enu(self, body_target_x, body_target_y, body_target_z): + + ENU_x = body_target_y + ENU_y = - body_target_x + ENU_z = body_target_z + + return ENU_x, ENU_y, ENU_z + + def body2enu_velocity(self, body_target_vx, body_target_vy, body_target_vz): + + ENU_vx = body_target_vy + ENU_vy = - body_target_vx + ENU_vz = body_target_vz + + return ENU_vx, ENU_vy, ENU_vz + + def BodyOffsetENU2FLU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + def BodyOffsetENU2FLU_Velocity(self, msg): + FLU_vx = msg.vector.x * math.cos(self.current_heading) - msg.vector.y * math.sin(self.current_heading) + FLU_vy = msg.vector.x * math.sin(self.current_heading) + msg.vector.y * math.cos(self.current_heading) + FLU_vz = msg.vector.z + + return FLU_vx, FLU_vy, FLU_vz + + def set_target_position_callback(self, msg): + self.flag = 0 + #print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_x, FLU_y, FLU_z = self.BodyOffsetENU2FLU(msg) + + body_x = FLU_x + self.local_pose.pose.position.x + body_y = FLU_y + self.local_pose.pose.position.y + body_z = FLU_z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_pose_target(x=body_x, + y=body_y, + z=body_z, + ) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_x, ENU_y, ENU_z = self.body2enu(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) + + self.cur_target_pose = self.construct_pose_target(x=ENU_x,y=ENU_y,z=ENU_z) + + + def set_target_velocity_callback(self, msg): + self.flag = 1 + #print("Received New Velocity Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_vx, FLU_vy, FLU_vz = self.BodyOffsetENU2FLU_Velocity(msg) + self.cur_target_twist.twist.linear.x=FLU_vx + self.cur_target_twist.twist.linear.y=FLU_vy + self.cur_target_twist.twist.linear.z=FLU_vz + #print(self.cur_target_pose) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_vx, ENU_vy, ENU_vz = self.body2enu_velocity(msg.x, msg.y, msg.z) + + #self.cur_target_pose = self.cur_target_pose(vx=ENU_vx,vy=ENU_vy,vz=ENU_vz) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=0.1, + ) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() + time.sleep(2) diff --git a/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav2.py b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav2.py new file mode 100644 index 00000000..8984b128 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav2.py @@ -0,0 +1,369 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 15 + self.local_enu_position = None + + self.cur_target_pose = PoseStamped() + self.cur_target_twist=TwistStamped() + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + self.flag = 0 + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav2/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav2/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav2/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav2/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_velocity_sub = rospy.Subscriber("/uav2/gi/set_pose/velocity", Vector3Stamped, self.set_target_velocity_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav2/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav2/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.pose_target_pub = rospy.Publisher('/uav2/mavros/setpoint_position/local', PoseStamped, queue_size=10) + self.twist_target_pub = rospy.Publisher('/uav2/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav2/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav2/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav2_offboard_node") + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, y=self.local_pose.pose.position.y, z=self.takeoff_height) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.pose_target_pub.publish(self.cur_target_pose) + self.twist_target_pub.publish(self.cur_target_twist) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + if(self.flag==0): + self.pose_target_pub.publish(self.cur_target_pose) + else: + self.twist_target_pub.publish(self.cur_target_twist) + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_pose_target(self, x=0, y=0, z=0): + target_raw_pose = PoseStamped() + target_raw_pose.header.stamp = rospy.Time.now() + target_raw_pose.pose.position.y = x + target_raw_pose.pose.position.y = y + target_raw_pose.pose.position.z = z + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + + def body2enu(self, body_target_x, body_target_y, body_target_z): + + ENU_x = body_target_y + ENU_y = - body_target_x + ENU_z = body_target_z + + return ENU_x, ENU_y, ENU_z + + def body2enu_velocity(self, body_target_vx, body_target_vy, body_target_vz): + + ENU_vx = body_target_vy + ENU_vy = - body_target_vx + ENU_vz = body_target_vz + + return ENU_vx, ENU_vy, ENU_vz + + def BodyOffsetENU2FLU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + def BodyOffsetENU2FLU_Velocity(self, msg): + FLU_vx = msg.vector.x * math.cos(self.current_heading) - msg.vector.y * math.sin(self.current_heading) + FLU_vy = msg.vector.x * math.sin(self.current_heading) + msg.vector.y * math.cos(self.current_heading) + FLU_vz = msg.vector.z + + return FLU_vx, FLU_vy, FLU_vz + + def set_target_position_callback(self, msg): + self.flag = 0 + #print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_x, FLU_y, FLU_z = self.BodyOffsetENU2FLU(msg) + + body_x = FLU_x + self.local_pose.pose.position.x + body_y = FLU_y + self.local_pose.pose.position.y + body_z = FLU_z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_pose_target(x=body_x, + y=body_y, + z=body_z, + ) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_x, ENU_y, ENU_z = self.body2enu(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) + + self.cur_target_pose = self.construct_pose_target(x=ENU_x,y=ENU_y,z=ENU_z) + + + def set_target_velocity_callback(self, msg): + self.flag = 1 + #print("Received New Velocity Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_vx, FLU_vy, FLU_vz = self.BodyOffsetENU2FLU_Velocity(msg) + self.cur_target_twist.twist.linear.x=FLU_vx + self.cur_target_twist.twist.linear.y=FLU_vy + self.cur_target_twist.twist.linear.z=FLU_vz + #print(self.cur_target_pose) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_vx, ENU_vy, ENU_vz = self.body2enu_velocity(msg.vector.x, msg.vector.y, msg.vector.z) + + #self.cur_target_pose = self.construct_twist_target(vx=ENU_vx,vy=ENU_vy,vz=ENU_vz) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=0.1, + ) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() + time.sleep(2) diff --git a/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav3.py b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav3.py new file mode 100644 index 00000000..f1b7888e --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav3.py @@ -0,0 +1,369 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 10 + self.local_enu_position = None + + self.cur_target_pose = PoseStamped() + self.cur_target_twist=TwistStamped() + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + self.flag = 0 + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav3/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav3/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav3/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav3/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav3/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_velocity_sub = rospy.Subscriber("/uav3/gi/set_pose/velocity", Vector3Stamped, self.set_target_velocity_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav3/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav3/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.pose_target_pub = rospy.Publisher('/uav3/mavros/setpoint_position/local', PoseStamped, queue_size=10) + self.twist_target_pub = rospy.Publisher('/uav3/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav3/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav3/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav3_offboard_node") + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, y=self.local_pose.pose.position.y, z=self.takeoff_height) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.pose_target_pub.publish(self.cur_target_pose) + self.twist_target_pub.publish(self.cur_target_twist) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + if(self.flag==0): + self.pose_target_pub.publish(self.cur_target_pose) + else: + self.twist_target_pub.publish(self.cur_target_twist) + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_pose_target(self, x=0, y=0, z=0): + target_raw_pose = PoseStamped() + target_raw_pose.header.stamp = rospy.Time.now() + target_raw_pose.pose.position.y = x + target_raw_pose.pose.position.y = y + target_raw_pose.pose.position.z = z + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + + def body2enu(self, body_target_x, body_target_y, body_target_z): + + ENU_x = body_target_y + ENU_y = - body_target_x + ENU_z = body_target_z + + return ENU_x, ENU_y, ENU_z + + def body2enu_velocity(self, body_target_vx, body_target_vy, body_target_vz): + + ENU_vx = body_target_vy + ENU_vy = - body_target_vx + ENU_vz = body_target_vz + + return ENU_vx, ENU_vy, ENU_vz + + def BodyOffsetENU2FLU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + def BodyOffsetENU2FLU_Velocity(self, msg): + FLU_vx = msg.vector.x * math.cos(self.current_heading) - msg.vector.y * math.sin(self.current_heading) + FLU_vy = msg.vector.x * math.sin(self.current_heading) + msg.vector.y * math.cos(self.current_heading) + FLU_vz = msg.vector.z + + return FLU_vx, FLU_vy, FLU_vz + + def set_target_position_callback(self, msg): + self.flag = 0 + #print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_x, FLU_y, FLU_z = self.BodyOffsetENU2FLU(msg) + + body_x = FLU_x + self.local_pose.pose.position.x + body_y = FLU_y + self.local_pose.pose.position.y + body_z = FLU_z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_pose_target(x=body_x, + y=body_y, + z=body_z, + ) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_x, ENU_y, ENU_z = self.body2enu(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) + + self.cur_target_pose = self.construct_pose_target(x=ENU_x,y=ENU_y,z=ENU_z) + + + def set_target_velocity_callback(self, msg): + self.flag = 1 + #print("Received New Velocity Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_vx, FLU_vy, FLU_vz = self.BodyOffsetENU2FLU_Velocity(msg) + self.cur_target_twist.twist.linear.x=FLU_vx + self.cur_target_twist.twist.linear.y=FLU_vy + self.cur_target_twist.twist.linear.z=FLU_vz + #print(self.cur_target_pose) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_vx, ENU_vy, ENU_vz = self.body2enu_velocity(msg.x, msg.y, msg.z) + + #self.cur_target_pose = self.construct_twist_target(vx=ENU_vx,vy=ENU_vy,vz=ENU_vz) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=0.1, + ) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() + time.sleep(2) diff --git a/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav4.py b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav4.py new file mode 100644 index 00000000..b802defc --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/px4_mavros_run_uav4.py @@ -0,0 +1,369 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3Stamped +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 5 + self.local_enu_position = None + + self.cur_target_pose = PoseStamped() + self.cur_target_twist=TwistStamped() + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + self.flag = 0 + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav4/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav4/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav4/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav4/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav4/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_velocity_sub = rospy.Subscriber("/uav4/gi/set_pose/velocity", Vector3Stamped, self.set_target_velocity_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav4/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav4/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.pose_target_pub = rospy.Publisher('/uav4/mavros/setpoint_position/local', PoseStamped, queue_size=10) + self.twist_target_pub = rospy.Publisher('/uav4/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav4/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav4/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav4_offboard_node") + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, y=self.local_pose.pose.position.y, z=self.takeoff_height) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.pose_target_pub.publish(self.cur_target_pose) + self.twist_target_pub.publish(self.cur_target_twist) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + if(self.flag==0): + self.pose_target_pub.publish(self.cur_target_pose) + else: + self.twist_target_pub.publish(self.cur_target_twist) + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_pose_target(self, x=0, y=0, z=0): + target_raw_pose = PoseStamped() + target_raw_pose.header.stamp = rospy.Time.now() + target_raw_pose.pose.position.y = x + target_raw_pose.pose.position.y = y + target_raw_pose.pose.position.z = z + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + + def body2enu(self, body_target_x, body_target_y, body_target_z): + + ENU_x = body_target_y + ENU_y = - body_target_x + ENU_z = body_target_z + + return ENU_x, ENU_y, ENU_z + + def body2enu_velocity(self, body_target_vx, body_target_vy, body_target_vz): + + ENU_vx = body_target_vy + ENU_vy = - body_target_vx + ENU_vz = body_target_vz + + return ENU_vx, ENU_vy, ENU_vz + + def BodyOffsetENU2FLU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + def BodyOffsetENU2FLU_Velocity(self, msg): + FLU_vx = msg.vector.x * math.cos(self.current_heading) - msg.vector.y * math.sin(self.current_heading) + FLU_vy = msg.vector.x * math.sin(self.current_heading) + msg.vector.y * math.cos(self.current_heading) + FLU_vz = msg.vector.z + + return FLU_vx, FLU_vy, FLU_vz + + def set_target_position_callback(self, msg): + self.flag = 0 + #print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_x, FLU_y, FLU_z = self.BodyOffsetENU2FLU(msg) + + body_x = FLU_x + self.local_pose.pose.position.x + body_y = FLU_y + self.local_pose.pose.position.y + body_z = FLU_z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_pose_target(x=body_x, + y=body_y, + z=body_z, + ) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_x, ENU_y, ENU_z = self.body2enu(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) + + self.cur_target_pose = self.construct_pose_target(x=ENU_x,y=ENU_y,z=ENU_z) + + + def set_target_velocity_callback(self, msg): + self.flag = 1 + #print("Received New Velocity Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_OFFSET_ENU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + #print("body FLU frame") + + FLU_vx, FLU_vy, FLU_vz = self.BodyOffsetENU2FLU_Velocity(msg) + self.cur_target_twist.twist.linear.x=FLU_vx + self.cur_target_twist.twist.linear.y=FLU_vy + self.cur_target_twist.twist.linear.z=FLU_vz + #print(self.cur_target_pose) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + #print("local ENU frame") + + ENU_vx, ENU_vy, ENU_vz = self.body2enu_velocity(msg.x, msg.y, msg.z) + + #self.cur_target_pose = self.construct_twist_target(vx=ENU_vx,vy=ENU_vy,vz=ENU_vz) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=0.1, + ) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_pose_target(x=self.local_pose.pose.position.x, + y=self.local_pose.pose.position.y, + z=self.local_pose.pose.position.z, + ) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() + time.sleep(2) diff --git a/demo/tutorial_7/multi_uavs_motion_planning/run.sh b/demo/tutorial_7/multi_uavs_motion_planning/run.sh new file mode 100644 index 00000000..540ad8cd --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/run.sh @@ -0,0 +1,9 @@ +python px4_mavros_run_uav1.py& +python px4_mavros_run_uav2.py& +python px4_mavros_run_uav3.py& +python px4_mavros_run_uav4.py& +sleep 10s +python commander_uav1.py& +python commander_uav2.py& +python commander_uav3.py& +python commander_uav4.py& diff --git a/demo/tutorial_7/multi_uavs_motion_planning/stop.sh b/demo/tutorial_7/multi_uavs_motion_planning/stop.sh new file mode 100644 index 00000000..592438a4 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_motion_planning/stop.sh @@ -0,0 +1,8 @@ +killall -9 python px4_mavros_run_uav1.py +killall -9 python px4_mavros_run_uav2.py +killall -9 python px4_mavros_run_uav3.py +killall -9 python px4_mavros_run_uav4.py +killall -9 python commander_uav1.py +killall -9 python commander_uav2.py +killall -9 python commander_uav3.py +killall -9 python commander_uav4.py diff --git a/demo/tutorial_7/multi_uavs_path_planning/commander_uav1.py b/demo/tutorial_7/multi_uavs_path_planning/commander_uav1.py new file mode 100644 index 00000000..b08cf437 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/commander_uav1.py @@ -0,0 +1,75 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav1_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav1/gi/set_pose/position', PoseStamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav1/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav1/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav1/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x, y, z, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_FLU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + if BODY_FLU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + + +if __name__ == "__main__": + + con = Commander() + time.sleep(2) + i=1 + while True: + con.move(50*math.cos(2*math.pi*i/100),50*math.sin(2*math.pi*i/100), con.local_pose.pose.position.z ,BODY_OFFSET_ENU=False) + if abs(50*math.cos(2*math.pi*i/100)-con.local_pose.pose.position.x)<2 and abs(50*math.sin(2*math.pi*i/100)-con.local_pose.pose.position.y)<2: + i=i+1 + time.sleep(0.02) \ No newline at end of file diff --git a/demo/tutorial_7/multi_uavs_path_planning/commander_uav2.py b/demo/tutorial_7/multi_uavs_path_planning/commander_uav2.py new file mode 100644 index 00000000..9e1ce160 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/commander_uav2.py @@ -0,0 +1,75 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav2_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav2/gi/set_pose/position', PoseStamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav2/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav2/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x, y, z, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_FLU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + if BODY_FLU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + + +if __name__ == "__main__": + + con = Commander() + time.sleep(2) + i=1 + while True: + con.move(-50*math.cos(2*math.pi*i/100),50*math.sin(2*math.pi*i/100), con.local_pose.pose.position.z ,BODY_OFFSET_ENU=False) + if abs(-50*math.cos(2*math.pi*i/100)-con.local_pose.pose.position.x)<2 and abs(50*math.sin(2*math.pi*i/100)-con.local_pose.pose.position.y)<2: + i=i+1 + time.sleep(0.02) \ No newline at end of file diff --git a/demo/tutorial_7/multi_uavs_path_planning/commander_uav3.py b/demo/tutorial_7/multi_uavs_path_planning/commander_uav3.py new file mode 100644 index 00000000..2330f1fe --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/commander_uav3.py @@ -0,0 +1,75 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav3_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav3/gi/set_pose/position', PoseStamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav3/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav3/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav3/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x, y, z, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_FLU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + if BODY_FLU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + + +if __name__ == "__main__": + + con = Commander() + time.sleep(2) + i=1 + while True: + con.move(-50*math.cos(2*math.pi*i/100),-50*math.sin(2*math.pi*i/100), con.local_pose.pose.position.z ,BODY_OFFSET_ENU=False) + if abs(-50*math.cos(2*math.pi*i/100)-con.local_pose.pose.position.x)<2 and abs(-50*math.sin(2*math.pi*i/100)-con.local_pose.pose.position.y)<2: + i=i+1 + time.sleep(0.02) \ No newline at end of file diff --git a/demo/tutorial_7/multi_uavs_path_planning/commander_uav4.py b/demo/tutorial_7/multi_uavs_path_planning/commander_uav4.py new file mode 100644 index 00000000..616ba0a0 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/commander_uav4.py @@ -0,0 +1,75 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, String +from pyquaternion import Quaternion +import time +import math + + +class Commander: + def __init__(self): + rospy.init_node("uav4_commander_node") + rate = rospy.Rate(20) + self.position_target_pub = rospy.Publisher('/uav4/gi/set_pose/position', PoseStamped, queue_size=10) + self.yaw_target_pub = rospy.Publisher('/uav4/gi/set_pose/orientation', Float32, queue_size=10) + self.custom_activity_pub = rospy.Publisher('/uav4/gi/set_activity/type', String, queue_size=10) + + self.local_pose_sub = rospy.Subscriber("/uav4/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + + def local_pose_callback(self, msg): + self.local_pose = msg + + def move(self, x, y, z, BODY_OFFSET_ENU=True): + self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU)) + + + def turn(self, yaw_degree): + self.yaw_target_pub.publish(yaw_degree) + + + # land at current position + def land(self): + self.custom_activity_pub.publish(String("LAND")) + + + # hover at current position + def hover(self): + self.custom_activity_pub.publish(String("HOVER")) + + + # return to home position with defined height + def return_home(self, height): + self.position_target_pub.publish(self.set_pose(0, 0, height, False)) + + + def set_pose(self, x=0, y=0, z=2, BODY_FLU = True): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + + # ROS uses ENU internally, so we will stick to this convention + if BODY_FLU: + pose.header.frame_id = 'base_link' + + else: + pose.header.frame_id = 'map' + + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + + return pose + + +if __name__ == "__main__": + + con = Commander() + time.sleep(2) + i=1 + while True: + con.move(50*math.cos(2*math.pi*i/100),-50*math.sin(2*math.pi*i/100), con.local_pose.pose.position.z ,BODY_OFFSET_ENU=False) + if abs(50*math.cos(2*math.pi*i/100)-con.local_pose.pose.position.x)<2 and abs(-50*math.sin(2*math.pi*i/100)-con.local_pose.pose.position.y)<2: + i=i+1 + time.sleep(0.02) \ No newline at end of file diff --git a/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav1.py b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav1.py new file mode 100644 index 00000000..caceba46 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav1.py @@ -0,0 +1,304 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 5 + self.local_enu_position = None + + self.cur_target_pose = None + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav1/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav1/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav1/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav1/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav1/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav1/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav1/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.local_target_pub = rospy.Publisher('/uav1/mavros/setpoint_raw/local', PositionTarget, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav1/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav1/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav1_offboard_node") + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, self.local_pose.pose.position.y, self.takeoff_height, self.current_heading) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.local_target_pub.publish(self.cur_target_pose) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + + self.local_target_pub.publish(self.cur_target_pose) + + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_target(self, x, y, z, yaw, yaw_rate = 1): + target_raw_pose = PositionTarget() + target_raw_pose.header.stamp = rospy.Time.now() + + target_raw_pose.coordinate_frame = 9 + + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z + + target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + + PositionTarget.FORCE + + target_raw_pose.yaw = yaw + target_raw_pose.yaw_rate = yaw_rate + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + def FLU2ENU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + + def set_target_position_callback(self, msg): + print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_FLU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + print("body FLU frame") + + ENU_X, ENU_Y, ENU_Z = self.FLU2ENU(msg) + + ENU_X = ENU_X + self.local_pose.pose.position.x + ENU_Y = ENU_Y + self.local_pose.pose.position.y + ENU_Z = ENU_Z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_target(ENU_X, + ENU_Y, + ENU_Z, + self.current_heading) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + print("local ENU frame") + + self.cur_target_pose = self.construct_target(msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z, + self.current_heading) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + 0.1, + self.current_heading) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + yaw_deg) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + self.current_heading) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() diff --git a/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav2.py b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav2.py new file mode 100644 index 00000000..a90f7716 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav2.py @@ -0,0 +1,304 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 10 + self.local_enu_position = None + + self.cur_target_pose = None + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav2/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav2/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav2/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav2/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav2/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav2/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.local_target_pub = rospy.Publisher('/uav2/mavros/setpoint_raw/local', PositionTarget, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav2/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav2/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav2_offboard_node") + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, self.local_pose.pose.position.y, self.takeoff_height, self.current_heading) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.local_target_pub.publish(self.cur_target_pose) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + + self.local_target_pub.publish(self.cur_target_pose) + + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_target(self, x, y, z, yaw, yaw_rate = 1): + target_raw_pose = PositionTarget() + target_raw_pose.header.stamp = rospy.Time.now() + + target_raw_pose.coordinate_frame = 9 + + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z + + target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + + PositionTarget.FORCE + + target_raw_pose.yaw = yaw + target_raw_pose.yaw_rate = yaw_rate + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + def FLU2ENU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + + def set_target_position_callback(self, msg): + print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_FLU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + print("body FLU frame") + + ENU_X, ENU_Y, ENU_Z = self.FLU2ENU(msg) + + ENU_X = ENU_X + self.local_pose.pose.position.x + ENU_Y = ENU_Y + self.local_pose.pose.position.y + ENU_Z = ENU_Z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_target(ENU_X, + ENU_Y, + ENU_Z, + self.current_heading) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + print("local ENU frame") + + self.cur_target_pose = self.construct_target(msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z, + self.current_heading) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + 0.1, + self.current_heading) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + yaw_deg) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + self.current_heading) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() diff --git a/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav3.py b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav3.py new file mode 100644 index 00000000..2aaafa6c --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav3.py @@ -0,0 +1,304 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 15 + self.local_enu_position = None + + self.cur_target_pose = None + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav3/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav3/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav3/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav3/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav3/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav3/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav3/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.local_target_pub = rospy.Publisher('/uav3/mavros/setpoint_raw/local', PositionTarget, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav3/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav3/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav3_offboard_node") + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, self.local_pose.pose.position.y, self.takeoff_height, self.current_heading) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.local_target_pub.publish(self.cur_target_pose) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + + self.local_target_pub.publish(self.cur_target_pose) + + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_target(self, x, y, z, yaw, yaw_rate = 1): + target_raw_pose = PositionTarget() + target_raw_pose.header.stamp = rospy.Time.now() + + target_raw_pose.coordinate_frame = 9 + + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z + + target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + + PositionTarget.FORCE + + target_raw_pose.yaw = yaw + target_raw_pose.yaw_rate = yaw_rate + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + def FLU2ENU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + + def set_target_position_callback(self, msg): + print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_FLU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + print("body FLU frame") + + ENU_X, ENU_Y, ENU_Z = self.FLU2ENU(msg) + + ENU_X = ENU_X + self.local_pose.pose.position.x + ENU_Y = ENU_Y + self.local_pose.pose.position.y + ENU_Z = ENU_Z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_target(ENU_X, + ENU_Y, + ENU_Z, + self.current_heading) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + print("local ENU frame") + + self.cur_target_pose = self.construct_target(msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z, + self.current_heading) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + 0.1, + self.current_heading) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + yaw_deg) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + self.current_heading) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() diff --git a/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav4.py b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav4.py new file mode 100644 index 00000000..62fba1ad --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/px4_mavros_run_uav4.py @@ -0,0 +1,304 @@ +import rospy +from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Imu, NavSatFix +from std_msgs.msg import Float32, Float64, String +import time +from pyquaternion import Quaternion +import math +import threading + + +class Px4Controller: + + def __init__(self): + + self.imu = None + self.gps = None + self.local_pose = None + self.current_state = None + self.current_heading = None + self.takeoff_height = 20 + self.local_enu_position = None + + self.cur_target_pose = None + self.global_target = None + + self.received_new_task = False + self.arm_state = False + self.offboard_state = False + self.received_imu = False + self.frame = "BODY" + + self.state = None + + ''' + ros subscribers + ''' + self.local_pose_sub = rospy.Subscriber("/uav4/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.mavros_sub = rospy.Subscriber("/uav4/mavros/state", State, self.mavros_state_callback) + self.gps_sub = rospy.Subscriber("/uav4/mavros/global_position/global", NavSatFix, self.gps_callback) + self.imu_sub = rospy.Subscriber("/uav4/mavros/imu/data", Imu, self.imu_callback) + + self.set_target_position_sub = rospy.Subscriber("/uav4/gi/set_pose/position", PoseStamped, self.set_target_position_callback) + self.set_target_yaw_sub = rospy.Subscriber("/uav4/gi/set_pose/orientation", Float32, self.set_target_yaw_callback) + self.custom_activity_sub = rospy.Subscriber("/uav4/gi/set_activity/type", String, self.custom_activity_callback) + + + ''' + ros publishers + ''' + self.local_target_pub = rospy.Publisher('/uav4/mavros/setpoint_raw/local', PositionTarget, queue_size=10) + + ''' + ros services + ''' + self.armService = rospy.ServiceProxy('/uav4/mavros/cmd/arming', CommandBool) + self.flightModeService = rospy.ServiceProxy('/uav4/mavros/set_mode', SetMode) + + + print("Px4 Controller Initialized!") + + + def start(self): + rospy.init_node("uav4_offboard_node") + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, self.local_pose.pose.position.y, self.takeoff_height, self.current_heading) + + #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose)) + + for i in range(20): + self.local_target_pub.publish(self.cur_target_pose) + self.arm_state = self.arm() + self.offboard_state = self.offboard() + time.sleep(0.2) + + + if self.takeoff_detection(): + print("Vehicle Took Off!") + + else: + print("Vehicle Took Off Failed!") + return + + ''' + main ROS thread + ''' + while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False): + + self.local_target_pub.publish(self.cur_target_pose) + + if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15): + + if(self.disarm()): + + self.state = "DISARMED" + + + time.sleep(0.1) + + + def construct_target(self, x, y, z, yaw, yaw_rate = 1): + target_raw_pose = PositionTarget() + target_raw_pose.header.stamp = rospy.Time.now() + + target_raw_pose.coordinate_frame = 9 + + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z + + target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + + PositionTarget.FORCE + + target_raw_pose.yaw = yaw + target_raw_pose.yaw_rate = yaw_rate + + return target_raw_pose + + + + ''' + cur_p : poseStamped + target_p: positionTarget + ''' + def position_distance(self, cur_p, target_p, threshold=0.1): + delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x) + delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y) + delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z) + + if (delta_x + delta_y + delta_z < threshold): + return True + else: + return False + + + def local_pose_callback(self, msg): + self.local_pose = msg + self.local_enu_position = msg + + + def mavros_state_callback(self, msg): + self.mavros_state = msg.mode + + + def imu_callback(self, msg): + global global_imu, current_heading + self.imu = msg + + self.current_heading = self.q2yaw(self.imu.orientation) + + self.received_imu = True + + + def gps_callback(self, msg): + self.gps = msg + + def FLU2ENU(self, msg): + + FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading) + FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading) + FLU_z = msg.pose.position.z + + return FLU_x, FLU_y, FLU_z + + + def set_target_position_callback(self, msg): + print("Received New Position Task!") + + if msg.header.frame_id == 'base_link': + ''' + BODY_FLU + ''' + # For Body frame, we will use FLU (Forward, Left and Up) + # +Z +X + # ^ ^ + # | / + # |/ + # +Y <------body + + self.frame = "BODY" + + print("body FLU frame") + + ENU_X, ENU_Y, ENU_Z = self.FLU2ENU(msg) + + ENU_X = ENU_X + self.local_pose.pose.position.x + ENU_Y = ENU_Y + self.local_pose.pose.position.y + ENU_Z = ENU_Z + self.local_pose.pose.position.z + + self.cur_target_pose = self.construct_target(ENU_X, + ENU_Y, + ENU_Z, + self.current_heading) + + + else: + ''' + LOCAL_ENU + ''' + # For world frame, we will use ENU (EAST, NORTH and UP) + # +Z +Y + # ^ ^ + # | / + # |/ + # world------> +X + + self.frame = "LOCAL_ENU" + print("local ENU frame") + + self.cur_target_pose = self.construct_target(msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z, + self.current_heading) + + ''' + Receive A Custom Activity + ''' + + def custom_activity_callback(self, msg): + + print("Received Custom Activity:", msg.data) + + if msg.data == "LAND": + print("LANDING!") + self.state = "LAND" + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + 0.1, + self.current_heading) + + if msg.data == "HOVER": + print("HOVERING!") + self.state = "HOVER" + self.hover() + + else: + print("Received Custom Activity:", msg.data, "not supported yet!") + + + def set_target_yaw_callback(self, msg): + print("Received New Yaw Task!") + + yaw_deg = msg.data * math.pi / 180.0 + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + yaw_deg) + + ''' + return yaw from current IMU + ''' + def q2yaw(self, q): + if isinstance(q, Quaternion): + rotate_z_rad = q.yaw_pitch_roll[0] + else: + q_ = Quaternion(q.w, q.x, q.y, q.z) + rotate_z_rad = q_.yaw_pitch_roll[0] + + return rotate_z_rad + + + def arm(self): + if self.armService(True): + return True + else: + print("Vehicle arming failed!") + return False + + def disarm(self): + if self.armService(False): + return True + else: + print("Vehicle disarming failed!") + return False + + + def offboard(self): + if self.flightModeService(custom_mode='OFFBOARD'): + return True + else: + print("Vechile Offboard failed") + return False + + + def hover(self): + + self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x, + self.local_pose.pose.position.y, + self.local_pose.pose.position.z, + self.current_heading) + + def takeoff_detection(self): + if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state: + return True + else: + return False + + +if __name__ == '__main__': + + con = Px4Controller() + con.start() diff --git a/demo/tutorial_7/multi_uavs_path_planning/run.sh b/demo/tutorial_7/multi_uavs_path_planning/run.sh new file mode 100644 index 00000000..540ad8cd --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/run.sh @@ -0,0 +1,9 @@ +python px4_mavros_run_uav1.py& +python px4_mavros_run_uav2.py& +python px4_mavros_run_uav3.py& +python px4_mavros_run_uav4.py& +sleep 10s +python commander_uav1.py& +python commander_uav2.py& +python commander_uav3.py& +python commander_uav4.py& diff --git a/demo/tutorial_7/multi_uavs_path_planning/stop.sh b/demo/tutorial_7/multi_uavs_path_planning/stop.sh new file mode 100644 index 00000000..592438a4 --- /dev/null +++ b/demo/tutorial_7/multi_uavs_path_planning/stop.sh @@ -0,0 +1,8 @@ +killall -9 python px4_mavros_run_uav1.py +killall -9 python px4_mavros_run_uav2.py +killall -9 python px4_mavros_run_uav3.py +killall -9 python px4_mavros_run_uav4.py +killall -9 python commander_uav1.py +killall -9 python commander_uav2.py +killall -9 python commander_uav3.py +killall -9 python commander_uav4.py diff --git a/simulator/launch/multi_uav_mavros_sitl.launch b/simulator/launch/multi_uav_mavros_sitl.launch new file mode 100644 index 00000000..c65f042f --- /dev/null +++ b/simulator/launch/multi_uav_mavros_sitl.launch @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/posix-config/iris_1 b/simulator/posix-config/iris_1 new file mode 100644 index 00000000..6079d6fb --- /dev/null +++ b/simulator/posix-config/iris_1 @@ -0,0 +1,81 @@ +uorb start +param load +dataman start +param set MAV_SYS_ID 1 +param set BAT_N_CELLS 3 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_ID 1310728 +param set CAL_ACC1_XOFF 0.01 +param set CAL_GYRO0_ID 2293768 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_MAG0_ID 196616 +param set CAL_MAG0_XOFF 0.01 +param set COM_DISARM_LAND 3 +param set COM_OBL_ACT 2 +param set COM_OBL_RC_ACT 0 +param set COM_OF_LOSS_T 5 +param set COM_RC_IN_MODE 1 +param set EKF2_AID_MASK 1 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_HGT_MODE 0 +param set EKF2_MAG_TYPE 1 +param set MAV_TYPE 2 +param set MC_PITCH_P 6 +param set MC_PITCHRATE_P 0.2 +param set MC_ROLL_P 6 +param set MC_ROLLRATE_P 0.2 +param set MIS_TAKEOFF_ALT 2.5 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_Z_VEL_I 0.15 +param set MPC_Z_VEL_P 0.6 +param set NAV_ACC_RAD 2.0 +param set NAV_DLL_ACT 2 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 +param set RTL_RETURN_ALT 30.0 +param set SDLOG_DIRS_MAX 7 +param set SENS_BOARD_ROT 0 +param set SENS_BOARD_X_OFF 0.000001 +param set SITL_UDP_PRT 14560 +param set SYS_AUTOSTART 4010 +param set SYS_MC_EST_GROUP 2 +param set SYS_RESTART_TYPE 2 +replay tryapplyparams +simulator start -s +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim start +sensors start +commander start +land_detector start multicopter +navigator start +ekf2 start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -x -u 14556 -r 4000000 +mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +logger start -e -t +mavlink boot_complete +replay trystart diff --git a/simulator/posix-config/iris_2 b/simulator/posix-config/iris_2 new file mode 100644 index 00000000..73f56db8 --- /dev/null +++ b/simulator/posix-config/iris_2 @@ -0,0 +1,81 @@ +uorb start +param load +dataman start +param set MAV_SYS_ID 2 +param set BAT_N_CELLS 3 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_ID 1310728 +param set CAL_ACC1_XOFF 0.01 +param set CAL_GYRO0_ID 2293768 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_MAG0_ID 196616 +param set CAL_MAG0_XOFF 0.01 +param set COM_DISARM_LAND 3 +param set COM_OBL_ACT 2 +param set COM_OBL_RC_ACT 0 +param set COM_OF_LOSS_T 5 +param set COM_RC_IN_MODE 1 +param set EKF2_AID_MASK 1 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_HGT_MODE 0 +param set EKF2_MAG_TYPE 1 +param set MAV_TYPE 2 +param set MC_PITCH_P 6 +param set MC_PITCHRATE_P 0.2 +param set MC_ROLL_P 6 +param set MC_ROLLRATE_P 0.2 +param set MIS_TAKEOFF_ALT 2.5 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_Z_VEL_I 0.15 +param set MPC_Z_VEL_P 0.6 +param set NAV_ACC_RAD 2.0 +param set NAV_DLL_ACT 2 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 +param set RTL_RETURN_ALT 30.0 +param set SDLOG_DIRS_MAX 7 +param set SENS_BOARD_ROT 0 +param set SENS_BOARD_X_OFF 0.000001 +param set SITL_UDP_PRT 14562 +param set SYS_AUTOSTART 4010 +param set SYS_MC_EST_GROUP 2 +param set SYS_RESTART_TYPE 2 +replay tryapplyparams +simulator start -s +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim start +sensors start +commander start +land_detector start multicopter +navigator start +ekf2 start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -x -u 14558 -r 4000000 +mavlink start -x -u 14559 -r 4000000 -m onboard -o 14541 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14558 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14558 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14558 +mavlink stream -r 50 -s ATTITUDE -u 14558 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14558 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14558 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14558 +mavlink stream -r 20 -s RC_CHANNELS -u 14558 +mavlink stream -r 250 -s HIGHRES_IMU -u 14558 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14558 +logger start -e -t +mavlink boot_complete +replay trystart diff --git a/simulator/posix-config/iris_3 b/simulator/posix-config/iris_3 new file mode 100644 index 00000000..a439b0a7 --- /dev/null +++ b/simulator/posix-config/iris_3 @@ -0,0 +1,81 @@ +uorb start +param load +dataman start +param set MAV_SYS_ID 3 +param set BAT_N_CELLS 3 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_ID 1310728 +param set CAL_ACC1_XOFF 0.01 +param set CAL_GYRO0_ID 2293768 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_MAG0_ID 196616 +param set CAL_MAG0_XOFF 0.01 +param set COM_DISARM_LAND 3 +param set COM_OBL_ACT 2 +param set COM_OBL_RC_ACT 0 +param set COM_OF_LOSS_T 5 +param set COM_RC_IN_MODE 1 +param set EKF2_AID_MASK 1 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_HGT_MODE 0 +param set EKF2_MAG_TYPE 1 +param set MAV_TYPE 2 +param set MC_PITCH_P 6 +param set MC_PITCHRATE_P 0.2 +param set MC_ROLL_P 6 +param set MC_ROLLRATE_P 0.2 +param set MIS_TAKEOFF_ALT 2.5 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_Z_VEL_I 0.15 +param set MPC_Z_VEL_P 0.6 +param set NAV_ACC_RAD 2.0 +param set NAV_DLL_ACT 2 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 +param set RTL_RETURN_ALT 30.0 +param set SDLOG_DIRS_MAX 7 +param set SENS_BOARD_ROT 0 +param set SENS_BOARD_X_OFF 0.000001 +param set SITL_UDP_PRT 14564 +param set SYS_AUTOSTART 4010 +param set SYS_MC_EST_GROUP 2 +param set SYS_RESTART_TYPE 2 +replay tryapplyparams +simulator start -s +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim start +sensors start +commander start +land_detector start multicopter +navigator start +ekf2 start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -x -u 14572 -r 4000000 +mavlink start -x -u 14573 -r 4000000 -m onboard -o 14542 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14572 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14572 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14572 +mavlink stream -r 50 -s ATTITUDE -u 14572 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14572 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14572 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14572 +mavlink stream -r 20 -s RC_CHANNELS -u 14572 +mavlink stream -r 250 -s HIGHRES_IMU -u 14572 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14572 +logger start -e -t +mavlink boot_complete +replay trystart diff --git a/simulator/posix-config/iris_4 b/simulator/posix-config/iris_4 new file mode 100644 index 00000000..f568be3b --- /dev/null +++ b/simulator/posix-config/iris_4 @@ -0,0 +1,81 @@ +uorb start +param load +dataman start +param set MAV_SYS_ID 4 +param set BAT_N_CELLS 3 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_ID 1310728 +param set CAL_ACC1_XOFF 0.01 +param set CAL_GYRO0_ID 2293768 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_MAG0_ID 196616 +param set CAL_MAG0_XOFF 0.01 +param set COM_DISARM_LAND 3 +param set COM_OBL_ACT 2 +param set COM_OBL_RC_ACT 0 +param set COM_OF_LOSS_T 5 +param set COM_RC_IN_MODE 1 +param set EKF2_AID_MASK 1 +param set EKF2_ANGERR_INIT 0.01 +param set EKF2_GBIAS_INIT 0.01 +param set EKF2_HGT_MODE 0 +param set EKF2_MAG_TYPE 1 +param set MAV_TYPE 2 +param set MC_PITCH_P 6 +param set MC_PITCHRATE_P 0.2 +param set MC_ROLL_P 6 +param set MC_ROLLRATE_P 0.2 +param set MIS_TAKEOFF_ALT 2.5 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_Z_VEL_I 0.15 +param set MPC_Z_VEL_P 0.6 +param set NAV_ACC_RAD 2.0 +param set NAV_DLL_ACT 2 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 +param set RTL_RETURN_ALT 30.0 +param set SDLOG_DIRS_MAX 7 +param set SENS_BOARD_ROT 0 +param set SENS_BOARD_X_OFF 0.000001 +param set SITL_UDP_PRT 14566 +param set SYS_AUTOSTART 4010 +param set SYS_MC_EST_GROUP 2 +param set SYS_RESTART_TYPE 2 +replay tryapplyparams +simulator start -s +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim start +sensors start +commander start +land_detector start multicopter +navigator start +ekf2 start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -x -u 14574 -r 4000000 +mavlink start -x -u 14575 -r 4000000 -m onboard -o 14543 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14574 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14574 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14574 +mavlink stream -r 50 -s ATTITUDE -u 14574 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14574 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14574 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14574 +mavlink stream -r 20 -s RC_CHANNELS -u 14574 +mavlink stream -r 250 -s HIGHRES_IMU -u 14574 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14574 +logger start -e -t +mavlink boot_complete +replay trystart