def main():
# initiate node
rospy.init_node('static_controller_node', anonymous=True)
# flight mode object
modes = set_point.fcuModes()
# flight Controller
cnt = Controller()
rate = rospy.Rate(20.0)
# Subscribe to drone state
rospy.Subscriber('mavros/state', State, cnt.stateCb)
# Subscribe to UAV and Payload Position
rospy.Subscriber('/gazebo/link_states', LinkStates, cnt.get_position)
# Attitude Setpoint publisher
at_pub = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
# Position Setpoint Publisher
pos_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
# Make sure the drone is armed
while not cnt.state.armed:
modes.setArm()
rate.sleep()
print('armed')
# We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
k = 0
while k<10:
pos_pub.publish(cnt.pos_sp)
rate.sleep()
k = k + 1
# activate OFFBOARD mode
modes.setOffboardMode()
# wait until the altitude of UAV is 5m
cnt.init_position()
while cnt.uav_pos.z<4.95 or cnt.uav_pos.x>2.05:
print("uav_pos_z:",cnt.uav_pos.z)
print("uav_x",cnt.uav_pos.x)
print("----------------")
pos_pub.publish(cnt.pos_sp)
print("reached")
time.sleep(0.1)
# ROS main loop
t_start = time.time()
while not rospy.is_shutdown():
t = time.time() - t_start
cnt.output(t)
at_pub.publish(cnt.sp)
print("uav_x",cnt.uav_pos.x)
print("uav_y",cnt.uav_pos.y)
print("uav_pos_z:",cnt.uav_pos.z)
print("----------------")
rate.sleep()
def main():
# initiate node
rospy.init_node('controller_node', anonymous=True)
# flight mode object
modes = set_point.fcuModes()
# flight Controller
cnt = Controller()
rate = rospy.Rate(20.0)
# Subscribe to drone state
rospy.Subscriber('mavros/state', State, cnt.stateCb)
# Subscribe to UAV and Payload Position
rospy.Subscriber('/gazebo/link_states', LinkStates, cnt.get_position)
def main():
# initiate node
rospy.init_node('odometry_node', anonymous=True)
# flight mode object
modes = set_point.fcuModes()
# flight Controller
cnt = Controller()
rate = rospy.Rate(20.0)
# Subscribe to drone state
rospy.Subscriber('mavros/state', State, cnt.stateCb)
# Subscribe to UAV Position
#rospy.Subscriber('mavros/local_position/pose', PoseStamped, cnt.posUAV)
# Subscribe to payload Position
rospy.Subscriber('/gazebo/link_states', LinkStates, cnt.posLoad)
# Setpoint publisher
sp_pub = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
print(cnt.state.armed)
# Make sure the drone is armed
while not cnt.state.armed:
modes.setArm()
rate.sleep()
print('armed')
# We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
k = 0
while k < 10:
sp_pub.publish(cnt.sp)
rate.sleep()
k = k + 1
# activate OFFBOARD mode
modes.setOffboardMode()
cnt.sp.type_mask = int('010111111000', 2)
while cnt.uav_pos.z < 5:
print("uav_pos_z:", cnt.uav_pos.z)
print("load_pos_z:", cnt.load_pos.z)
cnt.updateSp()
sp_pub.publish(cnt.sp)
print("reached")
# ROS main loop
cnt.sp.type_mask = int('010111100011', 2)
while not rospy.is_shutdown():
cnt.commander()
sp_pub.publish(cnt.sp)
if cnt.uav_pos.z > 1:
print("uav_x", cnt.uav_pos.x)
print("uav_y", cnt.uav_pos.y)
print("load_x", cnt.load_pos.x)
print("load_y", cnt.load_pos.y)
print("diff_x:", cnt.rel_pos.x)
print("diff_y:", cnt.rel_pos.y)
print("velocity_x:", cnt.sp.velocity.x)
print("velocity_y:", cnt.sp.velocity.y)
print("velocity_z:", cnt.sp.velocity.z)
print("force_x:", cnt.sp.acceleration_or_force.x)
print("force_y:", cnt.sp.acceleration_or_force.y)
print("force_z:", cnt.sp.acceleration_or_force.z)
print("----------------")
rate.sleep()