def set_mode(self, mode):
if not self.current_state.connected:
print "No FCU connection"
elif self.current_state.mode == mode:
pass
else:
# Request mode change with ros service
rospy.wait_for_service("mavros/set_mode")
set_mode = rospy.ServiceProxy("mavros/set_mode", SetMode)
req = SetModeRequest()
req.custom_mode = mode
while not rospy.is_shutdown() and (self.current_state.mode !=
req.custom_mode):
try:
# request
set_mode.call(req)
except rospy.ServiceException, e:
print "Service did not process request: %s" % str(e)
else:
return True
def set_mode(self, mode):
if not self.current_state.connected:
rospy.loginfo('Not Connected')
elif self.current_state.mode != mode:
rospy.wait_for_service('/mavros/set_mode')
set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)
req = SetModeRequest()
req.custom_mode = mode
pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 0
t0 = rospy.get_rostime() + rospy.Duration(3.0)
while not rospy.is_shutdown() and (self.current_state.mode !=
mode):
if rospy.get_rostime() - t0 > rospy.Duration(2.0):
try:
rospy.loginfo('Sending')
set_mode_client.call(req)
except rospy.ServiceException, e:
rospy.loginfo('Service did not process request: %s' %
str(e))
t0 = rospy.get_rostime()
pub.publish(pose)
self.rate.sleep()
rospy.loginfo(self.current_state.mode + ' Mode Esablished')
def arm(self):
# wait for connect
while not rospy.is_shutdown() and self.current_state == None:
rospy.loginfo("waiting for connection")
self.rate.sleep()
# must be streaming points before allowed to switch to offboard
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = self.des_z
for i in range(100):
self.local_pose_pub.publish(pose)
self.rate.sleep()
# change to offboard mode and arm
last_request = rospy.get_time()
# enable offboard mode
set_mode = rospy.ServiceProxy("%s/mavros/set_mode" % uav, SetMode)
req = SetModeRequest()
req.custom_mode = "OFFBOARD"
while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode):
self.local_pose_pub.publish(pose)
if rospy.get_time() - last_request > 5.0: # check every 5 seconds
try:
set_mode.call(req)
except rospy.ServiceException, e:
print "Service did not process request: %s"%str(e)
last_request = rospy.get_time()
self.rate.sleep()
def thread_offboard(self):
rospy.init_node('offboard', anonymous=True)
self.uav_id = rospy.get_param("~id", "")
rospy.loginfo(self.uav_id + ":start offboard control..")
rate = rospy.Rate(20)
rate.sleep()
rospy.Subscriber(self.uav_id + "/cmd/thrust", Float32,
self.thrustcallback)
rospy.Subscriber(self.uav_id + "/cmd/orientation", Quaternion,
self.quaternioncallback)
rospy.Subscriber(self.uav_id + "/mavros/state", State,
self.statecallback)
rospy.Subscriber(self.uav_id + "/mavros/local_position/pose",
PoseStamped, self.poscallback)
rospy.Subscriber(self.uav_id + "/mavros/local_position/velocity",
TwistStamped, self.velcallback)
arming_client = rospy.ServiceProxy(self.uav_id + "/mavros/cmd/arming",
CommandBool)
pub = rospy.Publisher(self.uav_id + "/mavros/setpoint_raw/attitude",
AttitudeTarget,
queue_size=20)
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
set_mode_client = rospy.ServiceProxy(self.uav_id + "/mavros/set_mode",
SetMode)
offb_set_mode = SetModeRequest()
offb_set_mode.base_mode = 0
offb_set_mode.custom_mode = "OFFBOARD"
now = rospy.get_rostime()
last_request = now.secs
while not (rospy.is_shutdown()):
if (self.targetattitude.thrust < -1):
rospy.loginfo(" error 00")
break
pub.publish(self.targetattitude)
if ((self.current_state.mode != "OFFBOARD")
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo(self.current_state.mode)
if ((set_mode_client.call(offb_set_mode) == True)
and (offb_set_mode.response.success == True)):
rospy.loginfo(self.uav_id + " offboard enabled")
last_request = rospy.get_rostime().secs
else:
if ((self.current_state.armed == False)
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo(self.uav_id + " arming...")
if (arming_client.call(arm_cmd).success):
rospy.loginfo(self.uav_id + " armed")
last_request = rospy.get_rostime().secs
rate.sleep()
def _handle_control_mode(self, mode):
"""
Handles the "set_contol_mode" service.
When called, places the FCU into manual flight mode and clears the last accel, vel, and pos setpoints.
"""
res = SetControlModeResponse()
res.success = False
if not self._ready:
return res
self._mode = mode.mode
req = SetModeRequest()
req.base_mode = 0
#req.custom_mode = "19" # MANUAL FLIGHT MODE
req.custom_mode = "0" # STABILIZE FLIGHT MODE
# req.custom_mode = "2" # ALT_HOLD FLIGHT MODE
self._configure_mavros.wait_for_service()
res.success = self._configure_mavros(req).mode_sent
self._control_depth = True
self._control_sway = True
self._control_yaw = True
self._latest_accel_sp = None
self._latest_vel_sp = None
self._latest_pos_sp = None
self._latest_ap_sp = None
self._latest_wp = None
return res
def request_mode(self, mode='OFFBOARD'):
rospy.sleep(2)
rospy.loginfo('Current mode: %s', self.state.mode)
req = SetModeRequest()
req.custom_mode = mode
if self.mode_client(req).mode_sent:
rospy.loginfo('Mode change request successful')
return True
else:
rospy.logwarn('Mode change request unsuccessful')
return False
def set_mode(self, new_mode):
"""
Функция отправки нового режима работы автопилота.
@param new_mode: новый режим работы атопилота
@type new_mode: String
"""
print("set mode")
mode = SetModeRequest()
mode.custom_mode = new_mode
# if self.diagnostics.mode != new_mode:
self.ws.call_service("mavros/set_mode", mode)
def main():
rospy.init_node('px4_offboard_python', anonymous=True)
#state_sub = rospy.Subscriber("mavros/state", )
#while not rospy.is_shutdown() and not current_state.connected:
# rospy.Rate(20)
#rospy.loginfo('FCU connection successful')
#rospy.Subscriber("chatter", String, callback)
pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
#local_pos_pub = get_pub_position_local(queue_size=10)
#arming_client = rospy.ServiceProxy("/mavros/arming", CommandBool)
set_mode = rospy.ServiceProxy("/mavros/set_mode", SetMode)
rate = rospy.Rate(20)
msg = PoseStamped()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = 0
msg.pose.position.y = 0
msg.pose.position.z = 0.5
#for i in range(100):
# local_pos_pub.Publish(msg)
# rate.sleep()
#offb_set_mode = SetMode()
#offb_set_mode.request.custom_mode('OFFBOARD')
#rospy.loginfo("Drone offboard!")
req = SetModeRequest()
req.custom_mode = 'OFFBOARD'
set_mode.call(req)
print 'mode: ' + State().mode
mavros.set_namespace()
command.arming(True)
rospy.loginfo("Drone armed!")
rospy.spin()
def set_mode(self, mode):
if not self.current_state.connected:
print "No FCU connection"
elif self.current_state.mode == mode:
print "Already in " + mode + " mode"
else:
# wait for service
rospy.wait_for_service("mavros/set_mode")
# service client
set_mode = rospy.ServiceProxy("mavros/set_mode", SetMode)
# set request object
req = SetModeRequest()
req.custom_mode = mode
# zero time
t0 = rospy.get_time()
# check response
while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode):
if rospy.get_time() - t0 > 2.0: # check every 5 seconds
try:
# request
set_mode.call(req)
except rospy.ServiceException, e:
print "Service did not process request: %s"%str(e)
t0 = rospy.get_time()
print "Mode: "+self.current_state.mode + " established"
# Wait for FCU to connect to MAVROS
while not rospy.is_shutdown() and not current_state.connected:
rate.sleep()
rospy.loginfo('FCU not connected')
rospy.loginfo('Publishing initial waypoints')
# Setpoints need to be streaming before commanding a switch to OFFBOARD Mode
for i in range(50):
if not rospy.is_shutdown():
pose_publisher.publish(PoseStamped())
rate.sleep()
offb_mode_req = SetModeRequest()
offb_mode_req.custom_mode = 'OFFBOARD'
arm_req = CommandBoolRequest()
arm_req.value = True
arm_client(arm_req)
last_req = rospy.Time.now()
while not rospy.is_shutdown():
while not current_state.armed and current_state.mode != 'OFFBOARD':
timer_cb(None)
rate.sleep()
rospy.Timer(rospy.Duration(5), timer_cb)
rospy.loginfo('Taking off')
takeoff(takeoff_client, 3)
time_start = rospy.Time.now()
def control():
# sp = np.load("xy_sin.npy")
state_sub = rospy.Subscriber("/mavros/state",
State,
state_cb,
queue_size=10)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
PoseStamped,
queue_size=10)
imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=10)
local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
lp_cb,
queue_size=10)
local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity",
TwistStamped,
lv_cb,
queue_size=10)
act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub",
ActuatorControl,
act_cb,
queue_size=10)
rospy.init_node('control', anonymous=True)
rate = rospy.Rate(50.0)
# print("*"*80)
while not rospy.is_shutdown() and not current_state.connected:
rate.sleep()
rospy.loginfo(current_state.connected)
# print("#"*80)
pose = PoseStamped()
# mocap_pose = PoseStamped()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time.now()
i = 0
act = ActuatorControl()
flag1 = False
flag2 = False
prev_imu_data = Imu()
prev_time = rospy.Time.now()
# prev_x = 0
# prev_y = 0
# prev_z = 0
# prev_vx = 0
# prev_vy = 0
# prev_vz = 0
prev_omega_x = 0
prev_omega_y = 0
prev_omega_z = 0
prev_vx = 0
prev_vy = 0
prev_vz = 0
delta_t = 0.02
count = 0
theta = 0.0
#rospy.loginfo("Outside")
r = np.random.rand(100000, 1) * 10
x_trgt = []
y_trgt = []
row = 0
theta = 0.0
x_step = 0.0
y_step = 0.0
for t in xrange(0, 100000):
val = r[t, 0]
for p in xrange(0, 50):
x_trgt.append(x_step)
y_trgt.append(r[t, 0] * mt.sin(theta) + y_step)
x_step += 0.01
y_step += 0.01
theta += 1.0 / 100
alpha = 0.01
temp = 0.0
y_new = []
for r_n in range(0, len(y_trgt)):
temp = temp + alpha * (y_trgt[r_n] - temp)
y_new.append(temp)
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" and (
rospy.Time.now() - last_request > rospy.Duration(5.0)):
offb_set_mode_response = set_mode_client(offb_set_mode)
if offb_set_mode_response.mode_sent:
rospy.loginfo("Offboard enabled")
flag1 = True
last_request = rospy.Time.now()
else:
if current_state.armed == False:
arm_cmd_response = arming_client(arm_cmd)
if arm_cmd_response.success:
rospy.loginfo("Vehicle armed")
flag2 = True
last_request = rospy.Time.now()
# rospy.loginfo("Inside")
curr_time = rospy.Time.now()
if flag1 and flag2:
x_f.append(float(local_position.pose.position.x))
y_f.append(float(local_position.pose.position.y))
z_f.append(float(local_position.pose.position.z))
vx_f.append(float(local_velocity.twist.linear.x))
vy_f.append(float(local_velocity.twist.linear.y))
vz_f.append(float(local_velocity.twist.linear.z))
# print(local_velocity.twist.linear.x)
orientation = [
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z
]
(roll, pitch, yaw) = quaternion_to_euler_angle(
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z)
r_f.append(float(mt.radians(roll)))
p_f.append(float(mt.radians(pitch)))
yaw_f.append(float(mt.radians(yaw)))
sin_r_f.append(mt.sin(float(mt.radians(roll))))
sin_p_f.append(mt.sin(float(mt.radians(pitch))))
sin_y_f.append(mt.sin(float(mt.radians(yaw))))
cos_r_f.append(mt.cos(float(mt.radians(roll))))
cos_p_f.append(mt.cos(float(mt.radians(pitch))))
cos_y_f.append(mt.cos(float(mt.radians(yaw))))
rs_f.append(float(imu_data.angular_velocity.x))
ps_f.append(float(imu_data.angular_velocity.y))
ys_f.append(float(imu_data.angular_velocity.z))
ix.append(float(ixx))
iy.append(float(iyy))
iz.append(float(izz))
m.append(float(mass))
u0.append(act_controls.controls[0])
u1.append(act_controls.controls[1])
u2.append(act_controls.controls[2])
u3.append(act_controls.controls[3])
pose.pose.position.x = y_new[row]
pose.pose.position.y = x_trgt[row]
pose.pose.position.z = 3
row = row + 1
x_des.append(0)
y_des.append(0)
z_des.append(3)
sin_y_des.append(yaw)
cos_y_des.append(yaw)
w_mag.append(0)
w_x.append(0)
w_y.append(0)
w_z.append(0)
n_t = curr_time - prev_time
#delta_t = float(n_t.nsecs) * 1e-9
a_x.append(
(float(local_velocity.twist.linear.x) - prev_vx) / delta_t)
a_y.append(
(float(local_velocity.twist.linear.y) - prev_vy) / delta_t)
a_z.append(
(float(local_velocity.twist.linear.z) - prev_vz) / delta_t)
prev_vx = float(local_velocity.twist.linear.x)
prev_vy = float(local_velocity.twist.linear.y)
prev_vz = float(local_velocity.twist.linear.z)
aplha_x.append(
(float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t)
aplha_y.append(
(float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t)
aplha_z.append(
(float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t)
ax_f.append(float(imu_data.linear_acceleration.x))
ay_f.append(float(imu_data.linear_acceleration.y))
az_f.append(float(imu_data.linear_acceleration.z))
prev_omega_x = float(imu_data.angular_velocity.x)
prev_omega_y = float(imu_data.angular_velocity.y)
prev_omega_z = float(imu_data.angular_velocity.z)
theta += 0.5
count += 1
local_pos_pub.publish(pose)
print('data_points =', data_points, " count = ", count, "row =",
row)
if (count >= data_points):
break
prev_time = curr_time
rate.sleep()
nn1_yaw_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u3
],
ndmin=2).transpose()
nn1_yaw_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose()
nn2_yaw_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u0, u1, u2
],
ndmin=2).transpose()
nn2_yaw_grd_truth = np.array([aplha_x, aplha_y, aplha_z],
ndmin=2).transpose()
print('nn1_x_rand_y_input_state :', nn1_yaw_input_state.shape)
print('nn1_x_rand_y_grd_truth :', nn1_yaw_grd_truth.shape)
print('nn2_x_rand_y_input_state :', nn2_yaw_input_state.shape)
print('nn2_x_rand_y_grd_truth :', nn2_yaw_grd_truth.shape)
np.save('nn1_x_rand_y_input_state.npy', nn1_yaw_input_state)
np.save('nn1_x_rand_y_grd_truth.npy', nn1_yaw_grd_truth)
np.save('nn2_x_rand_y_input_state.npy', nn2_yaw_input_state)
np.save('nn2_x_rand_y_grd_truth.npy', nn2_yaw_grd_truth)
s_x_rand_y = np.array([
x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, rs_f, ps_f, ys_f, r_f, p_f, yaw_f
],
ndmin=2).transpose()
u_x_rand_y = np.array([u0, u1, u2, u3], ndmin=2).transpose()
a_x_rand_y = np.array([ax_f, ay_f, az_f], ndmin=2).transpose()
print('s_x_rand_y :', s_x_rand_y.shape)
print('u_x_rand_y :', u_x_rand_y.shape)
print('a_x_rand_y :', a_x_rand_y.shape)
np.save('s_x_rand_y.npy', s_x_rand_y)
np.save('u_x_rand_y.npy', u_x_rand_y)
np.save('a_x_rand_y.npy', a_x_rand_y)
def run(self):
self.load_points()
rospy.loginfo('Points are loaded...')
rospy.Subscriber(self.prefix + '/mavros/state', State, self.state_cb)
rospy.loginfo("Drone" + str(self.drone) +
": Subscribed to the state topic")
rospy.Subscriber(self.prefix + '/mavros/local_position/pose',
PoseStamped, self.check_local_position)
local_pos_pub = rospy.Publisher(self.prefix +
'/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
rospy.wait_for_service(self.prefix + '/mavros/cmd/arming')
arming = rospy.ServiceProxy(self.prefix + '/mavros/cmd/arming',
CommandBool)
rospy.wait_for_service(self.prefix + '/mavros/set_mode')
set_mode = rospy.ServiceProxy(self.prefix + '/mavros/set_mode',
SetMode)
rate = rospy.Rate(20.0)
while (not rospy.is_shutdown()) and (not self.mavros_state.connected):
rospy.loginfo("Drone" + str(self.drone) +
": Waiting for a connection")
rospy.sleep(1)
for i in range(10):
local_pos_pub.publish(self.desired_point)
# rospy.loginfo(i)
rate.sleep()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time().now()
rospy.loginfo(last_request)
while (not rospy.is_shutdown()) and (not self.finish_mission):
if self.mavros_state.mode != 'OFFBOARD' and \
(rospy.Time().now() - last_request > rospy.Duration(5.0)):
response = set_mode(offb_set_mode)
if response.mode_sent:
rospy.loginfo("Drone" + str(self.drone) +
": Offboard enabled")
last_request = rospy.Time().now()
else:
if not self.mavros_state.armed and \
(rospy.Time().now() - last_request > rospy.Duration(5.0)):
response = arming(arm_cmd)
if response.success:
rospy.loginfo("Drone" + str(self.drone) +
": Vehicle armed")
last_request = rospy.Time().now()
local_pos_pub.publish(self.desired_point)
rate.sleep()
rospy.loginfo("Drone" + str(self.drone) + ": mission finished")
rospy.loginfo("Landing point is {}".format(self.land_point))
rospy.wait_for_service(self.prefix + "/mavros/cmd/land")
land = rospy.ServiceProxy(self.prefix + "/mavros/cmd/land", CommandTOL)
land(self.land_point)
self.result_queue.put((self.drone, self.liability))
def uav_takeoff(self):
rospy.init_node('takeoff', anonymous=True)
rate = rospy.Rate(20.0)
rate.sleep()
rospy.Subscriber('mavros/state', State, self.state_cb)
local_pose_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=150)
velocity_pub = rospy.Publisher(
'mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=10)
rospy.Subscriber('mavros/odometry/in', Odometry, self.odometry_cb)
home_position_pub = rospy.Publisher('mavros/home_position/home',
HomePosition,
queue_size=10)
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
setmode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
# rospy.spin()
# print 1
# wait for FCU connected
while (not rospy.is_shutdown()) and (not self.current_state.connected):
rate.sleep()
print("state connected")
# for i in range(100):
# if not rospy.is_shutdown():
# local_pose_pub.publish(self.pose)
# rate.sleep()
# print i
local_pose_pub.publish(self.pose)
takeoff_set_mode = SetModeRequest()
takeoff_set_mode.base_mode = 0
takeoff_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.get_rostime()
rospy.wait_for_service('mavros/set_mode')
print("initialized")
# print self.current_state.connected
# print self.current_state.mode
vel_mode = 0
self.vel = self.vel_change(vel_mode)
flag = 0
while not rospy.is_shutdown():
print("in loop")
if self.current_state.mode != "OFFBOARD" and (
rospy.get_rostime() - last_request) > rospy.Duration(3):
if setmode_client.call(takeoff_set_mode).mode_sent:
rospy.loginfo("offboard enabled")
last_request = rospy.get_rostime()
print self.current_state.mode
else:
if (
not self.current_state.armed
) and (rospy.get_rostime() - last_request) > rospy.Duration(3):
if arming_client.call(arm_cmd).success:
rospy.loginfo("Vehicle armd")
last_request = rospy.get_rostime()
if flag == 0:
if abs(self.current_odometry.pose.pose.position.z + 1) > 0.1:
local_pose_pub.publish(self.pose)
rate.sleep()
print(self.current_odometry.pose.pose.position.z)
if abs(self.current_odometry.pose.pose.position.z + 1) < 0.2:
flag = 1
if flag == 1:
for n in range(60): # delay 3.0s
velocity_pub.publish(self.vel)
print(self.vel.linear.x)
rate.sleep()
vel_mode = vel_mode + 1
if vel_mode == 5:
vel_mode = 1
self.vel = self.vel_change(vel_mode)
# tf axis has problem ,odo_x for setpoint_y and odo_y for setpoint_x
# error_x = abs(self.current_odometry.pose.pose.position.y - self.pose.pose.position.x)
# error_y = abs(self.current_odometry.pose.pose.position.x - self.pose.pose.position.y)
# error_z = abs(-self.current_odometry.pose.pose.position.z - self.pose.pose.position.z)
# error_sum = abs(error_x) + abs(error_y) + abs(error_z)
#
# home_position_pub.publish(self.home_position)
#
# if error_sum < 0.5:
# print "get point"+" "+str([self.pose.pose.position.x, self.pose.pose.position.y, self.pose.pose.position.z])
#
# self.sequence += 1
# self.get_waypoint(self.sequence, 5)
# self.set_newpoint(self.waypoint_x, self.waypoint_y, self.waypoint_z)
# for n in range(60): # delay 3.0s
# local_pose_pub.publish(self.pose)
# rate.sleep()
# elif error_sum >= 0.5:
# local_pose_pub.publish(self.pose)
rate.sleep()
def control():
# sp = np.load("xy_sin.npy")
state_sub = rospy.Subscriber("/mavros/state",
State,
state_cb,
queue_size=10)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
PoseStamped,
queue_size=10)
imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=1)
local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
lp_cb,
queue_size=1)
local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity",
TwistStamped,
lv_cb,
queue_size=1)
act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub",
ActuatorControl,
act_cb,
queue_size=1)
rospy.init_node('control', anonymous=True)
rate = rospy.Rate(50.0)
# print("*"*80)
while not rospy.is_shutdown() and not current_state.connected:
rate.sleep()
rospy.loginfo(current_state.connected)
# print("#"*80)
pose = PoseStamped()
# mocap_pose = PoseStamped()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time.now()
i = 0
act = ActuatorControl()
flag1 = False
flag2 = False
prev_imu_data = Imu()
prev_time = rospy.Time.now()
# prev_x = 0
# prev_y = 0
# prev_z = 0
# prev_vx = 0
# prev_vy = 0
# prev_vz = 0
prev_omega_x = 0
prev_omega_y = 0
prev_omega_z = 0
prev_vx = 0
prev_vy = 0
prev_vz = 0
delta_t = 0.02
count = 0
theta = 0.0
# rospy.loginfo("Outside")
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" and (
rospy.Time.now() - last_request > rospy.Duration(5.0)):
offb_set_mode_response = set_mode_client(offb_set_mode)
if offb_set_mode_response.mode_sent:
rospy.loginfo("Offboard enabled")
flag1 = True
last_request = rospy.Time.now()
else:
if current_state.armed == False:
arm_cmd_response = arming_client(arm_cmd)
if arm_cmd_response.success:
rospy.loginfo("Vehicle armed")
flag2 = True
last_request = rospy.Time.now()
# rospy.loginfo("Inside")
if flag1 and flag2:
x_f.append(float(local_position.pose.position.x))
y_f.append(float(local_position.pose.position.y))
z_f.append(float(local_position.pose.position.z))
vx_f.append(float(local_velocity.twist.linear.x))
vy_f.append(float(local_velocity.twist.linear.y))
vz_f.append(float(local_velocity.twist.linear.z))
# print(local_velocity.twist.linear.x)
orientation = [
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z
]
(roll, pitch, yaw) = quaternion_to_euler_angle(
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z)
r_f.append(float(mt.radians(roll)))
p_f.append(float(mt.radians(pitch)))
yaw_f.append(float(mt.radians(yaw)))
sin_r_f.append(mt.sin(float(mt.radians(roll))))
sin_p_f.append(mt.sin(float(mt.radians(pitch))))
sin_y_f.append(mt.sin(float(mt.radians(yaw))))
cos_r_f.append(mt.cos(float(mt.radians(roll))))
cos_p_f.append(mt.cos(float(mt.radians(pitch))))
cos_y_f.append(mt.cos(float(mt.radians(yaw))))
rs_f.append(float(imu_data.angular_velocity.x))
ps_f.append(float(imu_data.angular_velocity.y))
ys_f.append(float(imu_data.angular_velocity.z))
ix.append(float(ixx))
iy.append(float(iyy))
iz.append(float(izz))
m.append(float(mass))
u0.append(act_controls.controls[0])
u1.append(act_controls.controls[1])
u2.append(act_controls.controls[2])
u3.append(act_controls.controls[3])
pose.pose.position.x = theta
pose.pose.position.z = 6 + 2 * mt.sin(theta * PI / 6)
x_des.append(theta)
y_des.append(0)
z_des.append(6 + 2 * mt.sin(theta * PI / 6))
sin_y_des.append(yaw_des)
cos_y_des.append(yaw_des)
w_mag.append(0)
w_x.append(0)
w_y.append(0)
w_z.append(0)
a_x.append(
(float(local_velocity.twist.linear.x) - prev_vx) / delta_t)
a_y.append(
(float(local_velocity.twist.linear.y) - prev_vy) / delta_t)
a_z.append(
(float(local_velocity.twist.linear.z) - prev_vz) / delta_t)
prev_vx = float(local_velocity.twist.linear.x)
prev_vy = float(local_velocity.twist.linear.y)
prev_vz = float(local_velocity.twist.linear.z)
aplha_x.append(
(float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t)
aplha_y.append(
(float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t)
aplha_z.append(
(float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t)
prev_omega_x = float(imu_data.angular_velocity.x)
prev_omega_y = float(imu_data.angular_velocity.y)
prev_omega_z = float(imu_data.angular_velocity.z)
theta += 1.0 / 80
count += 1
local_pos_pub.publish(pose)
if (count >= data_points):
break
rate.sleep()
nn1_xz_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u3
],
ndmin=2).transpose()
nn1_xz_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose()
nn2_xz_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u0, u1, u2
],
ndmin=2).transpose()
nn2_xz_grd_truth = np.array([aplha_x, aplha_y, aplha_z],
ndmin=2).transpose()
print('nn1_xz_input_state :', nn1_xz_input_state.shape)
print('nn1_xz_grd_truth :', nn1_xz_grd_truth.shape)
print('nn2_xz_input_state :', nn2_xz_input_state.shape)
print('nn2_xz_grd_truth :', nn2_xz_grd_truth.shape)
np.save('nn1_80_sin_xz_input_state.npy', nn1_xz_input_state)
np.save('nn1_80_sin_xz_grd_truth.npy', nn1_xz_grd_truth)
np.save('nn2_80_sin_xz_input_state.npy', nn2_xz_input_state)
np.save('nn2_80_sin_xz_grd_truth.npy', nn2_xz_grd_truth)
s_xz_sin = np.array([
x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, rs_f, ps_f, ys_f
],
ndmin=2).transpose()
u_xz_sin = np.array([u0, u1, u2, u3], ndmin=2).transpose()
print('s_xz_sin :', s_xz_sin.shape)
print('u_xz_sin :', u_xz_sin.shape)
np.save('s_80_xz_sin.npy', s_xz_sin)
np.save('u_80_xz_sin.npy', u_xz_sin)
def uav_takeoff(self):
rospy.init_node('takeoff', anonymous=True)
rate = rospy.Rate(20.0)
rate.sleep()
rospy.Subscriber('mavros/state', State, self.state_cb)
local_pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=150)
rospy.Subscriber('mavros/odometry/in',Odometry,self.odometry_cb)
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
setmode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
# rospy.spin()
# print 1
# wait for FCU connected
while (not rospy.is_shutdown()) and (not self.current_state.connected):
rate.sleep()
# print 2
# for i in range(100):
# if not rospy.is_shutdown():
# local_pose_pub.publish(self.pose)
# rate.sleep()
# print i
local_pose_pub.publish(self.pose)
takeoff_set_mode = SetModeRequest()
takeoff_set_mode.base_mode = 0
takeoff_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.get_rostime()
rospy.wait_for_service('mavros/set_mode')
# print 3
# print self.current_state.connected
# print self.current_state.mode
while not rospy.is_shutdown():
if self.current_state.mode != "OFFBOARD" and (rospy.get_rostime() - last_request) > rospy.Duration(3):
if setmode_client.call(takeoff_set_mode).mode_sent:
rospy.loginfo("offboard enabled")
last_request = rospy.get_rostime()
print self.current_state.mode
else:
if (not self.current_state.armed) and (rospy.get_rostime()-last_request) > rospy.Duration(3):
if arming_client.call(arm_cmd).success:
rospy.loginfo("Vehicle armd")
last_request = rospy.get_rostime()
# tf axis has problem ,odo_x for setpoint_y and odo_y for setpoint_x
error_x = abs(self.current_odometry.pose.pose.position.y - self.pose.pose.position.x)
error_y = abs(self.current_odometry.pose.pose.position.x - self.pose.pose.position.y)
error_z = abs(-self.current_odometry.pose.pose.position.z - self.pose.pose.position.z)
error_sum = abs(error_x) + abs(error_y) + abs(error_z)
if error_sum < 0.5:
print "get point"+" "+str([self.pose.pose.position.x, self.pose.pose.position.y, self.pose.pose.position.z])
self.sequence += 1
self.get_waypoint(self.sequence, 5)
self.set_newpoint(self.waypoint_x, self.waypoint_y, self.waypoint_z)
for n in range(60): # delay 3.0s
local_pose_pub.publish(self.pose)
rate.sleep()
elif error_sum >= 0.5:
local_pose_pub.publish(self.pose)
rate.sleep()
def px4_teleop_key():
rospy.init_node("px4_teleop_key_pub", anonymous=True)
rospy.loginfo("Node Initialized")
state_sub = rospy.Subscriber("mavros/state", State, state_cb, queue_size=10)
pos_teleop_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
rospy.loginfo("Subscriber, Publisher and Service Clients are initilized")
rate = rospy.Rate(20.0)
while not rospy.is_shutdown() and current_state.connected:
rate.sleep()
pos_teleop_msg = PoseStamped()
pos_teleop_msg.header.stamp = rospy.Time.now()
pos_teleop_msg.pose.position.x = 0.
pos_teleop_msg.pose.position.y = 0.
pos_teleop_msg.pose.position.z = 2.0
for i in range(100):
pos_teleop_pub.publish(pos_teleop_msg)
rate.sleep()
set_mode_req = SetModeRequest()
set_mode_req.custom_mode = "OFFBOARD"
arm_cmd_req = CommandBoolRequest()
arm_cmd_req.value = True
while not set_mode_client(set_mode_req).success:
pass
rospy.loginfo("Offboard enabled")
while not arming_client(arm_cmd_req).success:
pass
rospy.loginfo("Vehicle armed")
pos_teleop_pub.publish(pos_teleop_msg)
try:
stdscr = curses.initscr()
curses.noecho()
curses.cbreak()
stdscr.keypad(1)
show_key_config(stdscr)
while not rospy.is_shutdown():
if not current_state.mode=="OFFBOARD":
set_mode_client(set_mode_req)
op = stdscr.getch()
publish_pos(pos_teleop_pub, pos_teleop_msg, op)
rate.sleep()
finally:
curses.nocbreak()
stdscr.keypad(0)
curses.echo()
curses.endwin()
local_pub_pos.pose.position.z = 2
count = 0
while not (rospy.is_shutdown() or count > 50):
pub_local_position.publish(local_pub_pos)
print("pub local position 100 ")
rate.sleep()
count += 1
"""change Mode"""
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode)
offb_set_mode = SetModeRequest()
offb_set_mode.base_mode = 0
offb_set_mode.custom_mode = "OFFBOARD"
now = rospy.get_rostime()
last_request = now.secs
while not (rospy.is_shutdown()):
# pub_gps_geo.publish(gps_pub_geo_msg)
# pub_gps_global.publish(gps_pub_msg)
pub_local_position.publish(local_pub_pos)
if((uav_state.mode != "OFFBOARD") and (rospy.get_rostime().secs-last_request > 2)):
rospy.loginfo(uav_state.mode)
if((set_mode_client.call(offb_set_mode) == True) and (offb_set_mode.response.mode_sent == True)):
rospy.loginfo(" offboard enabled")
last_request = rospy.get_rostime().secs
else:
def listener():
global current_state
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.loginfo("start listenning")
rate = rospy.Rate(100)
rate.sleep()
#rospy.Subscriber("/mavros/imu/data", Imu, callback)
#rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, callback)
rospy.Subscriber("/uav2/mavros/state", State, callback)
arming_client = rospy.ServiceProxy("/uav2/mavros/cmd/arming", CommandBool)
pub = rospy.Publisher("/uav2/mavros/setpoint_attitude/att_throttle",
Float64,
queue_size=10)
pub1 = rospy.Publisher("/uav2/mavros/setpoint_attitude/attitude",
PoseStamped,
queue_size=10)
#pub=rospy.Publisher("/uav2/mavros/setpoint_position/local",PoseStamped,queue_size=10)
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
pose = Float64()
pose.data = 0.6
att = PoseStamped()
att.pose.orientation.x = 0.5
att.pose.orientation.y = 0.0
att.pose.orientation.z = 0.0
att.pose.orientation.w = 0.866
'''
pose=PoseStamped()
pose.pose.position.x=0
pose.pose.position.y=0
pose.pose.position.z=1
'''
rospy.loginfo("running")
set_mode_client = rospy.ServiceProxy("/uav2/mavros/set_mode", SetMode)
offb_set_mode = SetModeRequest()
offb_set_mode.base_mode = 0
offb_set_mode.custom_mode = "OFFBOARD"
now = rospy.get_rostime()
last_request = now.secs
while not (rospy.is_shutdown()):
pub.publish(pose)
pub1.publish(att)
#rospy.loginfo(current_state.mode)
if ((current_state.mode != "OFFBOARD")
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo(current_state.mode)
rospy.loginfo("1")
if (set_mode_client.call(offb_set_mode).success):
#if(set_mode_client.call(offb_set_mode)==True and offb_set_mode.response.success==True):
rospy.loginfo("offbrd enabled")
last_request = rospy.get_rostime().secs
else:
if ((current_state.armed == False)
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo("2")
if (arming_client.call(arm_cmd).success):
rospy.loginfo("armed")
last_request = rospy.get_rostime().secs
rate.sleep()
def offboard_node():
rospy.init_node("offb_node")
r = rospy.Rate(20)
rospy.Subscriber("mavros/state", State, state_cb)
local_pos_pub = rospy.Publisher("mavros/setpoint_position/local",
PoseStamped,
queue_size=1000)
arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
while not rospy.is_shutdown() and not current_state.connected:
r.sleep()
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 2
for i in range(100):
local_pos_pub.publish(pose)
r.sleep()
if rospy.is_shutdown():
break
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time.now()
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" \
and (rospy.Time.now() - last_request > rospy.Duration(5)):
try:
offb_set_mode_resp = set_mode_client(offb_set_mode)
if offb_set_mode_resp.mode_sent:
rospy.loginfo("Offboard enabled")
except rospy.ServiceException as e:
rospy.logwarn(e)
last_request = rospy.Time.now()
else:
if not current_state.armed \
and (rospy.Time.now() - last_request > rospy.Duration(5)):
try:
arm_cmd_resp = arming_client(arm_cmd)
if arm_cmd_resp.success:
rospy.loginfo("Vehicle armed")
except rospy.ServiceException as e:
rospy.logwarn(e)
last_request = rospy.Time.now()
local_pos_pub.publish(pose)
r.sleep()