def pitch_camera(self):
pitch_actuator = ActuatorControl()
pitch_actuator.group_mix = 2
pitch_actuator.controls[1] = -0.7
import pdb
pdb.set_trace()
for jj in range(20):
self.uav_actuator.publish(pitch_actuator)
self.uav_set_mode(base_mode=0, custom_mode="OFFBOARD")
self.uav_arming(True)
self.r.sleep()
def __init__(self):
magnetmsg = ActuatorControl('header', 'group.mix', 3)
magnetmsg.controls[5] = 1.0
publish = rospy.Publisher('/mavros/actuator_controls', ActuatorControl, queue_size=10)
self.rate = rospy.Rate(10) # 10Hz
publish.publish(magnetmsg)
self.rate.sleep()
def main():
rospy.init_node('actuator_controller', anonymous=True)
rospy.Subscriber("/release_signal", String, callback)
pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10)
rate = rospy.Duration(1/5.0)
msg_out = ActuatorControl()
msg_out.group_mix = 2 # Use group 2 (auxilary controls)
msg_out.controls = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
if blue_open:
act_index = blue_servo - 1
msg_out.controls[act_index] = 1.0
else:
act_index = blue_servo - 1
msg_out.controls[act_index] = -1.0
if orange_open:
act_index = orange_servo - 1
msg_out.controls[act_index] = 1.0
else:
act_index = orange_servo - 1
msg_out.controls[act_index] = -1.0
msg_out.header.stamp = rospy.Time.now()
pub.publish(msg_out)
rospy.sleep(rate)
def motr(a):
rospy.init_node('motor', anonymous=True)
pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = ActuatorControl()
msg.header.frame_id = "motorcontrol"
msg.controls = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
msg.group_mix = 3
time.sleep(3)
print 1
pub.publish(msg)
time.sleep(3)
rospy.spin()
def __init__(self):
self.message_pub = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=1)
self.actuator_control_message = ActuatorControl()
self.seq = 0
self.start_offboard()
self.start_throttle()
self.start_stearing()
def __init__(self):
self.pub_sp = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=10)
rospy.init_node('motorControl', anonymous=True)
rate = rospy.Rate(60) # 10hz
rospy.wait_for_service('mavros/set_mode')
change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
result_mode = change_mode(0, "MANUAL")
act = ActuatorControl()
while not rospy.is_shutdown():
act.controls = [1800, 1800, 1800, 1800, 0, 0, 0, 0]
rospy.loginfo(str(act.controls))
self.pub_sp.publish(act)
rate.sleep()
def talker():
rospy.init_node('actuator_controller', anonymous=True)
sub_trig = rospy.Subscriber('~imagery_trigger', Empty, talker)
pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10)
rate = rospy.Rate(1)
msg_out = ActuatorControl()
msg_out.group_mix = 1 # Use group 1 (auxilary controls)
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
is_low = True
if not rospy.is_shutdown():
is_low = not is_low
if is_low:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
rospy.loginfo("Set servos low")
else:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0]
rospy.loginfo("Set servos high")
msg_out.header.stamp = rospy.Time.now()
pub.publish(msg_out)
rate.sleep()
def listener():
# 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)
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch = roslaunch.parent.ROSLaunchParent(
uuid, ["/home/indigo/src/Firmware/launch/px4_posix_sitl.launch"])
launch.start()
pub = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=10)
rate = rospy.Rate(200) # 10hz
set_offboard_mode()
# next_step()
# next_step()
delay()
arm_quad()
# next_step()
# next_step()
while not rospy.is_shutdown():
# delay()
actuatorControl = ActuatorControl()
actuatorControl.group_mix = actuatorControl.PX4_MIX_MANUAL_PASSTHROUGH
actuatorControl.controls = [0, 0, 0, 1, 0, 0, 0, 0]
pub.publish(actuatorControl)
# next_step()
rate.sleep()
def set_act(self, group, output, freq):
"""
Offboard method that sets the values of the mixers and/or actuators of the vehicle.
Parameters
----------
group: int
Desired control group.
output : list
Desired output values for the mixers and/or actuators of the drone.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns + '/mavros/actuator_control',
ActuatorControl,
queue_size=1)
msg = ActuatorControl()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.group_mix = group
msg.controls = output
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def __init__(self, gui_ready):
self.gladefile = "gui.glade"
self.builder = Gtk.Builder()
self.builder.add_from_file(self.gladefile)
self.builder.connect_signals(self)
self.window = self.builder.get_object("main_window")
self.window.show()
self.lbl_airspeed = self.builder.get_object("lbl_airspeed")
self.lbl_groundspeed = self.builder.get_object("lbl_groundspeed")
self.lbl_heading = self.builder.get_object("lbl_heading")
self.lbl_throttle = self.builder.get_object("lbl_throttle")
self.lbl_altitude = self.builder.get_object("lbl_altitude")
self.lbl_climb = self.builder.get_object("lbl_climb")
self.lbl_landing_length = self.builder.get_object("lbl_landing_length")
self.lbl_rssi = self.builder.get_object("lbl_rssi")
self.lbl_bat_percent = self.builder.get_object("lbl_bat_percent")
self.lbl_payload0_info = self.builder.get_object("lbl_payload0_info")
self.lbl_payload1_info = self.builder.get_object("lbl_payload1_info")
self.btn_record_telemetry = self.builder.get_object("btn_record_telemetry")
self.btn_drop_payload0 = self.builder.get_object("btn_drop_payload0")
self.btn_drop_payload1 = self.builder.get_object("btn_drop_payload1")
self.btn_reset_das = self.builder.get_object("btn_reset_das")
self.btn_record_telemetry.set_label("Record Telemetry")
self.btn_video_window = self.builder.get_object("btn_video_window")
self.btn_video_window.set_label("Open FPV Window")
self.btn_record_video = self.builder.get_object("btn_record_video")
self.btn_record_video.set_label("Record Video")
self.reset_dialog = self.builder.get_object("reset_dialog")
self.reset_dialog.set_transient_for(self.window)
self.btn_reset_dialog_cncl = self.builder.get_object("btn_reset_dialog_cncl")
self.btn_reset_dialog_ok = self.builder.get_object("btn_reset_dialog_ok")
self.payload_dialog = self.builder.get_object("payload_dialog")
self.payload_dialog.set_transient_for(self.window)
self.lbl_payload_dialog = self.builder.get_object("lbl_payload_dialog")
self.btn_payload_dialog_cncl = self.builder.get_object("btn_payload_dialog_cncl")
self.btn_payload_dialog_ok = self.builder.get_object("lbl_payload_dialog_ok")
self.gui_ready = gui_ready
self.btn_tilt_up = self.builder.get_object("btn_tilt_up")
self.btn_tilt_down = self.builder.get_object("btn_tilt_down")
self.pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=3)
self.msg = ActuatorControl()
self.msg.header.frame_id = "payload_servo"
self.msg.group_mix = 3
def __init__(
self,
roll_degs,
pitch_degs,
yaw_degs,
):
self.message_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.actuator_control_message = ActuatorControl()
self.seq = 0
self.local_position = PoseStamped()
self.heading = 0.0
self.height = 0.0
self.roll_rads = np.deg2rad(roll_degs)
self.pitch_rads = -0.5 #np.derad(-.0) #np.deg2rad(pitch_degs)
self.yaw_rads = 0.0 # np.deg2rad(0.0) #np.deg2rad(yaw_degs)
def deploy_blue(self):
pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10)
rate = rospy.Rate(1)
msg_out = ActuatorControl()
msg_out.group_mix = 1 # Use group 1 (auxilary controls)
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
is_low = True
if not rospy.is_shutdown():
is_low = not is_low
if is_low:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
rospy.loginfo("Set servos low")
else:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 1.0]
rospy.loginfo("Set servos high")
msg_out.header.stamp = rospy.Time.now()
pub.publish(msg_out)
def __init__(self):
self.wins = 0
self.bounds_counter = 1
self.z_limit = 3
self.x_limit = 3
self.y_limit = 3
self.ang_vel_limit = 3
self.ang_pos_limit = 0.4
### define gym space ###
self.min_action = np.array([0, -1, -1, -1])
self.max_action = np.array([1, 1, 1, 1])
self.min_lin_pos = np.array([0.1, -20, -20])
self.max_lin_pos = np.array([40, 20, 20])
self.min_lin_vel = 10 * np.array([-1, -1, -1])
self.max_lin_vel = 10 * np.array([1, 1, 1])
self.min_lin_accl = 30 * np.array([-1, -1, -1])
self.max_lin_accl = 30 * np.array([1, 1, 1])
self.min_ang_pos = np.array([-1.57, -1.57, -1.57])
self.max_ang_pos = np.array([1.57, 1.57, 1.57])
self.min_ang_vel = 10 * np.array([-1, -1, -1])
self.max_ang_vel = 10 * np.array([1, 1, 1])
# self.low_state = np.array([ self.min_lin_pos, self.min_lin_vel, self.min_lin_accl, self.min_ang_pos, self.min_ang_vel ]).flatten()
# self.high_state = np.array([self.max_lin_pos, self.max_lin_vel, self.max_lin_accl, self.max_ang_pos, self.max_ang_vel ]).flatten()
self.low_state = np.array([
self.min_lin_pos, self.min_lin_vel, self.min_lin_accl,
self.min_ang_pos, self.min_ang_vel
]).flatten()
self.high_state = np.array([
self.max_lin_pos, self.max_lin_vel, self.max_lin_accl,
self.max_ang_pos, self.max_ang_vel
]).flatten()
self.action_space = spaces.Box(low=self.min_action,
high=self.max_action,
dtype=np.float32)
self.observation_space = spaces.Box(low=self.low_state,
high=self.high_state,
dtype=np.float32)
self.current_state = State()
self.imu_data = Imu()
self.act_controls = ActuatorControl()
self.pose = PoseStamped()
self.mocap_pose = PoseStamped()
self.thrust_ctrl = Thrust()
self.attitude_target = AttitudeTarget()
self.local_velocity = TwistStamped()
self.global_velocity = TwistStamped()
self.ground_truth = ModelStates()
self.local_accel = Imu()
# rospy.signal_shutdown('Resrating rospy')
# subprocess.Popen(args='make px4_sitl gazebo', cwd='../Firmware', shell=True, start_new_session=True)
# subprocess.Popen(args='roslaunch mavros px4.launch fcu_url:="udp://:[email protected]:14557"', cwd='../Firmware', shell=True, start_new_session=True)
### define ROS messages ###
self.offb_set_mode = SetModeRequest()
self.offb_set_mode.custom_mode = "OFFBOARD"
self.arm_cmd = CommandBoolRequest()
self.arm_cmd.value = True
self.disarm_cmd = CommandBoolRequest()
self.disarm_cmd.value = False
## ROS Subscribers
self.state_sub = rospy.Subscriber("/mavros/state",
State,
self.state_cb,
queue_size=10)
self.imu_sub = rospy.Subscriber("/mavros/imu/data",
Imu,
self.imu_cb,
queue_size=10)
self.local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
self.lp_cb,
queue_size=10)
self.local_vel_sub = rospy.Subscriber(
"/mavros/local_position/velocity_body",
TwistStamped,
self.lv_cb,
queue_size=10)
self.local_acc_sub = rospy.Subscriber("/mavros/imu/data",
Imu,
self.lacc_cb,
queue_size=10)
self.act_control_sub = rospy.Subscriber(
"/mavros/act_control/act_control_pub",
ActuatorControl,
self.act_cb,
queue_size=10)
self.global_alt_sub = rospy.Subscriber(
"/mavros/global_position/rel_alt",
Float64,
self.ra_cb,
queue_size=10)
self.global_pos_sub = rospy.Subscriber(
"/mavros/global_position/gp_vel",
TwistStamped,
self.gv_cb,
queue_size=10)
self.ground_truth_sub = rospy.Subscriber("/gazebo/model_states",
ModelStates,
self.gt_cb,
queue_size=10)
## ROS Publishers
self.local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
self.mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
PoseStamped,
queue_size=10)
self.acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
self.setpoint_raw_pub = rospy.Publisher(
"/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10)
self.thrust_pub = rospy.Publisher("/mavros/setpoint_attitude/thrust",
Thrust,
queue_size=10)
## initiate gym enviornment
self.init_env()
## ROS Services
rospy.wait_for_service('mavros/cmd/arming')
self.arming_client = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
rospy.wait_for_service('mavros/set_mode')
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.wait_for_service('mavros/set_stream_rate')
set_stream_rate = rospy.ServiceProxy("mavros/set_stream_rate",
StreamRate)
set_stream_rate(StreamRateRequest.STREAM_POSITION, 100, 1)
set_stream_rate(StreamRateRequest.STREAM_ALL, 100, 1)
self.setpoint_msg = mavros.setpoint.PoseStamped(
header=mavros.setpoint.Header(frame_id="att_pose",
stamp=rospy.Time.now()), )
self.offb_arm()
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)
mavros_msgs.srv.CommandBool)
armService(True)
except rospy.ServiceException, e:
print "Service arm call failed: %s" % e
rospy.wait_for_service('/mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('/mavros/set_mode',
mavros_msgs.srv.SetMode)
#http://wiki.ros.org/mavros/CustomModes for custom modes
isModeChanged = flightModeService(
custom_mode='OFFBOARD') #return true or false
except rospy.ServiceException, e:
print "service set_mode call failed: %s. GUIDED Mode could not be set. Check that GPS is enabled" % e
act = ActuatorControl()
act.header = Header()
act.controls[0] = 0.0 #ros_roll
act.controls[1] = 0.0 #ros_pitch
act.controls[2] = 0.0 + 0.2 #ros_yaw+offset
act.controls[3] = 0.0 #ros_throttle
act.controls[4] = 0.0
act.controls[5] = 0.0
act.controls[6] = 0.0
act.controls[7] = 0.0
while not rospy.is_shutdown():
pub_act.publish(act)
act.controls[2] = 0.0 + 0.2 #ros_yaw+offset
act.controls[3] = 0.5 #ros_throttle
rate.sleep()
def my_code():
act = ActuatorControl()
act.header = Header()
act.controls[0] = 0.0 #ros_roll
act.controls[1] = 0.0 #ros_pitch
act.controls[2] = 1 #ros_yaw
act.controls[3] = 0.5 #ros_throttle
act.controls[4] = 0.0
act.controls[5] = 0.0
act.controls[6] = 0.0
act.controls[7] = 0.0
while not rospy.is_shutdown():
pub_act.publish(act)
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)
# Prepare service calls
#arm_cmd = CommandBool()
#arm_cmd.request.value = True
#mode_cmd = SetMode()
#mode_cmd
# Prepare dummy pose sp
#send a few setpoints before starting
#pList = 10*[PoseStamped()]
pose_sp = PoseStamped()
pose_sp.pose.position.x = 5
pose_sp.pose.position.y = 2
pose_sp.pose.position.z = 2
actuator_sp = ActuatorControl()
actuator_sp.controls[5] = 0.5
actuator_sp.group_mix = ActuatorControl.PX4_MIX_MANUAL_PASSTHROUGH
i = 50
while (not rospy.is_shutdown()) and i > 0:
pub_pose_sp.publish(pose_sp)
i = i - 1
rate.sleep()
# Loop until ROS Shutdown
last_request = time.time()
while not rospy.is_shutdown():
#rospy.loginfo('state: ' + cur_state.mode)
if (cur_state.mode != "OFFBOARD" and (time.time() - last_request > 1)):
rospy.loginfo('attempting OFFBOARD')
def __init__(self):
### define gym spaces ###
self.min_action = 0.0
self.max_action = 1.0
self.min_position = 0.1
self.max_position = 25
self.max_speed = 3
self.low_state = np.array([self.min_position, -self.max_speed])
self.high_state = np.array([self.max_position, self.max_speed])
# self.low_state = np.array([self.min_position])
# self.high_state = np.array([self.max_position])
self.action_space = spaces.Box(low=self.min_action,
high=self.max_action,
shape=(1, ),
dtype=np.float32)
self.observation_space = spaces.Box(low=self.low_state,
high=self.high_state,
dtype=np.float32)
self.current_state = State()
self.imu_data = Imu()
self.act_controls = ActuatorControl()
self.pose = PoseStamped()
self.mocap_pose = PoseStamped()
self.thrust_ctrl = Thrust()
self.attitude_target = AttitudeTarget()
self.local_velocity = TwistStamped()
self.global_velocity = TwistStamped()
### define ROS messages ###
self.offb_set_mode = SetModeRequest()
self.offb_set_mode.custom_mode = "OFFBOARD"
self.arm_cmd = CommandBoolRequest()
self.arm_cmd.value = True
self.disarm_cmd = CommandBoolRequest()
self.disarm_cmd.value = False
## ROS Subscribers
self.state_sub = rospy.Subscriber("/mavros/state",
State,
self.state_cb,
queue_size=10)
self.imu_sub = rospy.Subscriber("/mavros/imu/data",
Imu,
self.imu_cb,
queue_size=10)
self.local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
self.lp_cb,
queue_size=10)
self.local_vel_sub = rospy.Subscriber(
"/mavros/local_position/velocity_local",
TwistStamped,
self.lv_cb,
queue_size=10)
self.act_control_sub = rospy.Subscriber(
"/mavros/act_control/act_control_pub",
ActuatorControl,
self.act_cb,
queue_size=10)
self.global_alt_sub = rospy.Subscriber(
"/mavros/global_position/rel_alt",
Float64,
self.ra_cb,
queue_size=10)
self.global_pos_sub = rospy.Subscriber(
"/mavros/global_position/gp_vel",
TwistStamped,
self.gv_cb,
queue_size=10)
## ROS Publishers
self.local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
self.mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
PoseStamped,
queue_size=10)
self.acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
self.setpoint_raw_pub = rospy.Publisher(
"/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10)
self.thrust_pub = rospy.Publisher("/mavros/setpoint_attitude/thrust",
Thrust,
queue_size=10)
## initiate gym enviornment
self.init_env()
## ROS Services
rospy.wait_for_service('mavros/cmd/arming')
self.arming_client = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
rospy.wait_for_service('mavros/set_mode')
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.wait_for_service('mavros/set_stream_rate')
set_stream_rate = rospy.ServiceProxy("mavros/set_stream_rate",
StreamRate)
set_stream_rate(StreamRateRequest.STREAM_POSITION, 50, 1)
set_stream_rate(StreamRateRequest.STREAM_ALL, 50, 1)
self.setpoint_msg = mavros.setpoint.PoseStamped(
header=mavros.setpoint.Header(frame_id="att_pose",
stamp=rospy.Time.now()), )
self.offb_arm()
def actuator_control(U):
actuator_publisher = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=10)
act_msg = ActuatorControl()
rate = rospy.Rate(100) # 10hz
act_msg.controls[0] = 0.0
act_msg.controls[1] = 0.0
act_msg.controls[2] = 0.0
act_msg.controls[3] = 0.0
act_msg.controls[4] = 0.0
act_msg.controls[5] = 0.0
act_msg.controls[6] = 0.0
act_msg.controls[7] = 0.0
if U[0] > 0.7:
U[0] = 0.7
act_msg.header.stamp = rospy.Time.now()
act_msg.group_mix = 0
if not U[0] == 0 or not U[1] == 0:
act_msg.controls[0] = U[1] - 0.2
act_msg.controls[3] = U[0]
else:
act_msg.controls[0] = 0.0
act_msg.controls[3] = 0.0
act_msg.controls[1] = 0.0
act_msg.controls[2] = 0.0
#act_msg.controls[3] = 0.5
act_msg.controls[4] = 0.0
act_msg.controls[5] = 0.0
act_msg.controls[6] = 0.0
act_msg.controls[7] = 0.0
actuator_publisher.publish(act_msg)
rate.sleep()
from mavros_msgs.srv import SetModeRequest
from mavros_msgs.srv import SetModeResponse
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import CommandBoolRequest
from mavros_msgs.srv import CommandBoolResponse
from sensor_msgs.msg import Imu
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("data_points", help="data_points", type=int)
args = parser.parse_args()
data_points = args.data_points
current_state = State()
imu_data = Imu()
act_controls = ActuatorControl()
PI = 3.14
k_p_yaw = 0.05
k_d_yaw = 0
k_p_pitch = 0.18
k_d_pitch = 0.045
k_p_roll = 0.18
k_d_roll = 0.045
x_f = []
y_f = []
z_f = []
def img_callback(img_msg):
# publish actuator steering with inference result if in OFFBOARD MODE
global offboard_mode_is_active
if offboard_mode_is_active:
global model
global graph
global pub_act_ctl
# preprocess img as in ML pipeline with PIL
# img = np.array(Image.frombytes('RGB',(160,120),io.BytesIO(img_msg.data),'raw'))
img = np.array(Image.open(io.BytesIO(img_msg.data)))
# img_arr = np.frombuffer(img_msg.data, dtype='int8').reshape(160, 120, 3)
# img inference
#deb = rospy.Time.now()
with graph.as_default():
#probs = model.predict(img_arr.reshape((1,) + img_arr.shape))
probs = model.predict(img.reshape((1, ) + img.shape))
#print(rospy.Time.now()-deb)
# process classes to output [-1,1] from 15 classes array
bucket_no = np.argmax(probs)
act_val = (bucket_no / 7) - 1.
# publish value in actuator_output_msg.channels[1] and fix power in actuator_output_msg.channels[1]
actuator_control = ActuatorControl()
actuator_control.header.stamp = rospy.Time.now()
actuator_control.group_mix = 0
actuator_control.controls[0] = 0.0
actuator_control.controls[1] = 0.0
actuator_control.controls[2] = act_val
actuator_control.controls[3] = 0.2 # Valeur entre 0 et 1
actuator_control.controls[4] = 0.0
actuator_control.controls[5] = 0.0
actuator_control.controls[6] = 0.0
actuator_control.controls[7] = 0.0
pub_act_ctl.publish(actuator_control)
def __init__(self):
self.node_name = rospy.get_name()
# Publicaiton
self.cmd_pub = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=1)
self.servo_state = 0
if __name__ == '__main__':
rospy.init_node('release_servo_test_node', anonymous=False)
rate = rospy.Rate(1) # 10hz
n = release_test()
n.servo_state = 0
cmd = ActuatorControl()
while not rospy.is_shutdown():
if n.servo_state == 0:
cmd.group_mix = 1
cmd.controls[4] = 1
cmd.controls[5] = 1
n.cmd_pub.publish(cmd)
n.servo_state = 1
else:
cmd.group_mix = 1
cmd.controls[4] = -1
cmd.controls[5] = -1
n.cmd_pub.publish(cmd)
setTakeoffMode()
elif (x == '6'):
setLandMode()
elif (x == '7'):
global latitude
global longitude
print("longitude: %.7f" % longitude)
print("latitude: %.7f" % latitude)
elif (x == '8'):
my_code()
else:
print "Exit"
if __name__ == '__main__':
rospy.init_node('my_mav', anonymous=True)
rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix,
globalPositionCallback)
velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
pub_act = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=10)
act = ActuatorControl()
# spin() simply keeps python from exiting until this node is stopped
#listener()
myLoop()
#rospy.spin()
