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 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 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 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"
def __init__(self):
self.home_gps_pub = rospy.Publisher('/plane_home_gps',
NavSatFix,
queue_size=1)
self.plane_local_pos_pub = rospy.Publisher(
'/mavros/setpoint_position/local', PoseStamped, queue_size=1)
self.start_trace_pub = rospy.Publisher('/start_trace',
Bool,
queue_size=1)
self.change_attitude_pub = rospy.Publisher('/change_to_attitude',
Bool,
queue_size=1)
self.plane_target_pos = PoseStamped()
self.plane_curr = PoseStamped()
self.car_pos = PoseStamped()
self.init_hover_mode = True
self.change_attitude = Bool()
self.follow_mode = Bool()
self.start_trace_msg = Bool()
self.fail_safe_mode = False
self.car_distance_x = 5
self.init_x = 0
self.init_y = 0
self.init_z = 3
self.destination_x = 0
self.destination_y = 0
self.delta_x = 0
self.delta_y = 0
self.points_num_x = 0 # need how many points to reach pre-position
self.points_num_y = 0 # need how many points to reach pre-position
self.point_x = 0
self.point_y = 0
self.step_x = 0
self.step_y = 0
self.traj_points_count = 0 # increase count in while loop to pub trajectory points
self.waypoints_num = 0
self.waypoints_list_x = list()
self.waypoints_list_y = list()
self.plan_enable = True
self.plan_num = 0
""">>>>>>test in gazebo """
self.uav_state = State()
self.arm_cmd = CommandBoolRequest()
rospy.Subscriber('/mavros/state', State, self.state_cb)
self.arming_client = rospy.ServiceProxy("/mavros/cmd/arming",
CommandBool)
self.set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode)
self.offb_set_mode = SetModeRequest()
"""<<<<<<test in gazebo """
def __init__(self, num):
if num < 0 or num > 80:
print "Error: initializing ", num, " drone. Bad number"
self.shutdown_hook()
#topics
self.state_sub = rospy.Subscriber('/uav' + str(num) + '/mavros/state',
State, self.state_cb)
self.position_sub = rospy.Subscriber(
'/uav' + str(num) + '/mavros/local_position/pose', PoseStamped,
self.pose_cb)
self.velocity_pub = rospy.Publisher(
'/uav' + str(num) + '/mavros/setpoint_velocity/cmd_vel_unstamped',
Twist,
queue_size=1)
self.position_pub = rospy.Publisher('uav' + str(num) +
'/mavros/setpoint_position/local',
PoseStamped,
queue_size=1)
self.arming_client = rospy.ServiceProxy(
'/uav' + str(num) + '/mavros/cmd/arming', CommandBool)
self.mode_client = rospy.ServiceProxy(
'/uav' + str(num) + '/mavros/set_mode', SetMode)
self.command_sub = rospy.Subscriber(
'/uav' + str(num) + '/mavros/command', Int32MultiArray,
self.cmd_cb)
self.sensor_sub = rospy.Subscriber(
'/uav' + str(num) + '/mavros/sensor', Float32MultiArray,
self.sensor_cb)
#variables
self.velocity_obj = Twist()
self.position_obj = PoseStamped()
self.position_send_obj = PoseStamped()
self.offb_srv_msg = SetModeRequest()
self.arming_srv_msg = CommandBoolRequest()
self.state = State()
self.state.connected = False
self.last_time = rospy.Time.now()
self.rate = rospy.Rate(20)
self.helmet_flag = False
self.object_flag = False
self.ctrl_c = False
self.obstacle = False
self.height = 2.5
#shutdown
rospy.on_shutdown(self.shutdownhook)
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self._srv_type == SetMode:
Logger.loginfo("In the if statement")
Logger.loginfo("Executing The service %s and sending " %
self._topic)
self.service_client.call(self._topic,
SetModeRequest(0, self._target))
else:
Logger.loginfo("Executing The service %s and sending " %
self._topic)
self.service_client.call(self._topic, self._target)
return 'Done' # One of the outcomes declared above.
def set_mode(self, mode):
"""This function changes the mode of the drone to a user specified mode. This takes the mode as a string. Ex. set_mode("GUIDED").
Args:
mode (String): Can be set to modes given in https://ardupilot.org/copter/docs/flight-modes.html
Returns:
0 (int): Mode Set successful.
-1 (int): Mode Set unsuccessful.
"""
SetMode_srv = SetModeRequest(0, mode)
response = self.set_mode_client(SetMode_srv)
if response.mode_sent:
rospy.loginfo(CGREEN2 + "SetMode Was successful" + CEND)
return 0
else:
rospy.logerr(CRED2 + "SetMode has failed" + CEND)
return -1
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()
state_sub = rospy.Subscriber('/uav0/mavros/state', State, state_cb)
state_msg = State()
# local position subscriber
pose_sub = rospy.Subscriber('/uav0/mavros/local_position/pose', PoseStamped,
pose_cb)
pose_msg = PoseStamped()
# velocity publisher
velocity_pub = rospy.Publisher(
'/uav0/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1)
velocity_msg = Twist()
# set_mode service
set_mode_srv = rospy.ServiceProxy('/uav0/mavros/set_mode', SetMode)
set_mode_msg = SetModeRequest()
# arm service
arming_srv = rospy.ServiceProxy('/uav0/mavros/cmd/arming', CommandBool)
arming_msg = CommandBoolRequest()
# rest of initialization
ctrl_c = False
rospy.on_shutdown(shutdownhook)
rate = rospy.Rate(20)
######## set up flight ########
# wait for connection to drone
while not ctrl_c and not state_msg.connected:
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 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 _start_mission(self):
print(self.namespace, 'waypoints')
req = WaypointPushRequest()
wp1 = Waypoint()
wp1.frame = Waypoint.FRAME_GLOBAL
wp1.command = CommandCode.NAV_TAKEOFF
wp1.is_current = True
wp1.autocontinue = True
wp1.param1 = 0.3 # minimum / desired pitch
wp1.param2 = 0.0
wp1.param3 = 0.0
wp1.param4 = 0.0 if self.color == 'blue' else -math.pi # yaw angle
wp1.x_lat = self.start_position.latitude
wp1.y_long = self.start_position.longitude
wp1.z_alt = self.start_position.altitude + ALTITUDE_ABOVE_GROUND
wp2 = copy.deepcopy(wp1)
wp2.command = CommandCode.NAV_WAYPOINT
wp2.is_current = False
wp2.param1 = 0.0
wp2.param2 = 5.0 # acceptance radius
wp2.param4 = 0.0
wp2.x_lat, wp2.y_long = cube_to_global(400.0, 250.0)
wp3 = copy.deepcopy(wp2)
wp3.autocontinue = False
wp3.x_lat, wp3.y_long = cube_to_global(100.0, 250.0)
req.waypoints = [wp1, wp2, wp3]
service_name = '%s/mavros/mission/push' % self.namespace
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, WaypointPush)
resp = service.call(req)
except rospy.ServiceException as e:
print(self.namespace,
'service call to push waypoints failed:',
str(e),
file=sys.stderr)
return False
if not resp.success:
print(self.namespace, 'failed to push waypoints', file=sys.stderr)
return False
print(self.namespace, 'pushed waypoints')
print(self.namespace, 'start mission')
req = SetModeRequest()
req.base_mode = 0 # use custom mode
# first 0x04: mode auto
# second 0x04: mission
# third and fourth 0x0000: empty
req.custom_mode = str(int('0x04040000', 0))
service_name = '%s/mavros/set_mode' % self.namespace
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, SetMode)
resp = service.call(req)
except rospy.ServiceException as e:
print(self.namespace,
'service call to set mode failed:',
str(e),
file=sys.stderr)
return False
if not resp.success:
print(self.namespace, 'failed to set mode', file=sys.stderr)
return False
print(self.namespace, 'started mission')
self._set_state(STATE_MISSION)
return True
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()
local_pub_pos.pose.position.x = 0
local_pub_pos.pose.position.y = 0
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
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()
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 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()
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 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 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)
rate = rospy.Rate(20)
# 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)
def run(self):
parser = argparse.ArgumentParser(
description='Swarm control',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
commands = ['arm', 'disarm', 'land', 'offboard', 'takeoff']
parser.add_argument('command', choices=commands, help='Command')
parser.add_argument('agent',
type=int,
nargs='?',
default=None,
help='Agent ID')
args = parser.parse_args()
master = masterapi.Master('/')
system_state = master.getSystemState()
self.services = [service[0] for service in system_state[2]]
self.topics = [t for t, _ in rospy.get_published_topics()]
if args.command == 'arm':
self.call_service('mavros/cmd/arming',
service_class=CommandBool,
request=CommandBoolRequest(True),
agent=args.agent,
function=self._are_agents_on_ground)
elif args.command == 'disarm':
self.call_service('mavros/cmd/arming',
service_class=CommandBool,
request=CommandBoolRequest(False),
agent=args.agent,
function=self._are_agents_on_ground)
elif args.command == 'land':
self.call_service('mavros/cmd/land',
service_class=CommandTOL,
request=CommandTOLRequest(),
agent=args.agent)
elif args.command == 'offboard':
self.call_service('mavros/set_mode',
service_class=SetMode,
request=SetModeRequest(custom_mode='OFFBOARD'),
agent=args.agent)
elif args.command == 'takeoff':
# Takeoff is a combination of takeoff and arming!
n = rospy.get_param('/gcs/n')
altitude = rospy.get_param('/gcs/origin_altitude')
agents = [args.agent] if args.agent is not None else list(
range(1, n + 1))
for agent in agents:
template = '/drone_{}/mavros/home_position/home'
home_position = rospy.wait_for_message(template.format(agent),
HomePosition)
request = CommandTOLRequest()
request.latitude = home_position.geo.latitude
request.longitude = home_position.geo.longitude
request.altitude = altitude
request.yaw = 0.5 * np.pi
self.call_service('mavros/cmd/takeoff',
service_class=CommandTOL,
request=request,
agent=agent)
self.call_service('mavros/cmd/arming',
service_class=CommandBool,
request=CommandBoolRequest(True),
agent=args.agent,
function=self._are_agents_on_ground)