def hover_rec(self, time_flying):
recursions = self.calculate_recursions(time_flying)
print(recursions)
for i in range(recursions):
print(i)
print(self.down_sensor_distance)
if self.beh_type == 'HOVER' and (self.hover_sensor_altitude_max >=
self.down_sensor_distance >=
self.hover_sensor_altitude_min):
print("The drone is hovering")
self.beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) #was 0.005 (now 50hz ,500loops)
elif self.beh_type == 'HOVER' and (
self.hover_sensor_altitude_max <=
self.down_sensor_distance): # We have to go down
print("Recovering hover position - going down")
self.beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust - self.Deaccumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif self.beh_type == 'HOVER' and (
self.down_sensor_distance <=
self.hover_sensor_altitude_min): # We have to go up
print("Recovering hover position - going up")
self.beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust + self.Deaccumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
print("time of hovering has ended")
self.beh_type = "LANDING"
self.landing_rec()
def set_att_thrust(self, att, att_type, thrust, freq):
"""
Offboard method that sends attitude and thrust references to the PX4 autopilot of the the vehicle.
Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS.
Converts the thrust from newtons to a normalized value between 0 and 1 through mathematical expression of the thrust curve of the vehicle.
Parameters
----------
att : np.array of floats with shape (3,1) or with shape (4,1)
Desired attitude for the vehicle, expressed in euler angles or in a quaternion.
att_type : str
Must be equal to either 'euler' or 'quaternion'. Specifies the format of the desired attitude.
thrust : float
Desired thrust value in newtons.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
msg = AttitudeTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
if att_type == 'euler':
msg.orientation = Quaternion(*quaternion_from_euler(
*ned_to_enu(att)))
elif att_type == 'quaternion':
msg.orientation = Quaternion(*SI_quaternion_to_ROS_quaternion(att))
msg.thrust = normalize_thrust(thrust, self.info.thrust_curve,
norm(self.ekf.vel))
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def secure_landing_phase_rec(self): # Secure landing part - last cm
self.beh_type = "LANDING"
recursions = self.calculate_recursions(self.Secure_time_landing)
thrust = self.hover_thrust
for i in range(recursions):
if thrust <= 0:
break
elif i < 200:
print("Secure hover before landing")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif i < 700:
print("Smooth landing")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust - self.Deaccumulating_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
else:
print("Landing")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust - self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
self.disarm()
def main1():
global setpoint
# thrust_pub = rospy.Publisher('/mavros/setpoint_attitude/thrust',Thrust,queue_size=10)
# thrust = Thrust()
attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=10)
point_pub = rospy.Publisher('/waypoint',PoseStamped,queue_size=10)
attitude = AttitudeTarget()
attitude.type_mask = 3
attitude.header = msg.header
# attitude.header.frame_id = 'FRAME_LOCAL_NED'
# quaternion = tf.transformations.quaternion_from_euler(msg.roll,msg.pitch, 0)
# attitude.orientation.x = quaternion[0]
# attitude.orientation.y = quaternion[1]
# attitude.orientation.z = quaternion[2]
# attitude.orientation.w = quaternion[3]
attitude.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(msg.roll, msg.pitch, 0))
attitude.body_rate.z = msg.yaw_rate
t = msg.thrust.z/100
if t>1:
t=1
elif t<-1:
t=-1
attitude.thrust = (t+1)/2
attitude_pub.publish(attitude)
point_pub.publish(setpoint)
def construct_target_attitude(self, body_x = 0, body_y = 0, body_z = 0, thrust=0):
target_raw_attitude = AttitudeTarget() # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = body_x # ROLL_RATE
target_raw_attitude.body_rate.y = body_y # PITCH_RATE
target_raw_attitude.body_rate.z = body_z # YAW_RATE
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
def landing_rec(self, thrust, beh_type, time_flying):
if (time_flying <= 0 or self.down_sensor_distance <=
self.landing_sensor_altitude_min) and beh_type == "LANDING":
print("Entering secure landing")
return self.secure_landing_phase_rec(thrust, beh_type,
self.Secure_time_landing)
elif thrust > self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust - self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.landing_rec(
thrust, beh_type,
time_flying) #bublishing a constant parameter "not
elif thrust <= self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = self.Landing_thrust
thrust = self.Landing_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.landing_rec(
thrust, beh_type, time_flying
) #bublishing a constant parameter "not updating thrust argument"
def lift_off_rec(self, thrust, beh_type, time_flying):
if (time_flying <= 0 or self.down_sensor_distance <=
self.hover_sensor_altitude_min) and beh_type == "TAKE OFF":
print("time of lift off has ended")
beh_type = "HOVER"
return self.hover_rec(thrust, beh_type, self.Hover_time)
elif beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min:
print("The drone is lifting off at constant thrust")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.Liftoff_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(
self.Time_between_messages) #was 0.005 (now 50hz ,500loops)
time_flying = time_flying - self.Time_between_messages
return self.lift_off_rec(target_raw_attitude.thrust, beh_type,
time_flying)
elif beh_type == "TAKE OFF":
print("Lifting the drone up slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust + self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.lift_off_rec(target_raw_attitude.thrust, beh_type,
time_flying)
def landing_rec(self): # Landing phase
self.beh_type = "LANDING"
recursions = self.calculate_recursions(self.Max_time_landing)
print(recursions)
thrust = self.hover_thrust
for i in range(recursions):
print self.down_sensor_distance
if self.down_sensor_distance <= self.landing_sensor_altitude_min:
break
elif thrust > self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust - self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif thrust <= self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = self.Landing_thrust
thrust = self.Landing_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
self.secure_landing_phase_rec2()
def lift_off_rec(self, thrust, time_flying):
recursions = self.calculate_recursions(time_flying)
print(recursions)
self.beh_type = "TAKE OFF"
for i in range(recursions):
print(i)
if self.beh_type == "TAKE OFF" and thrust < self.Liftoff_thrust: #and beh_type == "HOVER":
print("Lifting the drone up slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust + self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif self.beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min:
print("The drone is lifting off at constant thrust")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.Liftoff_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) #was 0.005 (now 50hz ,500loops)
else:
break
self.beh_type = 'HOVER'
print("time of lifting off has ended")
self.hover_rec(self.hover_time)
def secure_landing_phase_rec(self, thrust, time_flying):
if time_flying <= 0:
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = self.Landing_thrust
thrust = self.Landing_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.secure_landing_phase_rec(
thrust, beh_type, time_flying
) #bublishing a constant parameter "not updating thrust argument"
def main(args):
global list_current_position, list_current_velocity, current_position, usingvelocitycontrol, usingpositioncontrol, xvel, yvel, state, cur_pose, list_error, target
global global_obs_detected, list_velocity_angle, vController
par.CurrentIteration = 1
if (len(args) > 1):
uav_ID = str(args[1])
else:
uav_ID = "1"
idx = int(uav_ID) - 1
rospy.init_node("control_uav_" + uav_ID)
print "UAV" + uav_ID
vController = VelocityController()
try:
rospy.wait_for_service("/uav" + uav_ID + "/mavros/cmd/arming")
rospy.wait_for_service("/uav" + uav_ID + "/mavros/set_mode")
except rospy.ROSException:
fail("failed to connect to service")
state_sub = rospy.Subscriber("/uav" + uav_ID + "/mavros/state",
State,
queue_size=10,
callback=state_cb)
local_vel_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=10)
local_pos_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
local_pos_target = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/local",
PositionTarget,
queue_size=10)
atittude_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/attitude",
AttitudeTarget,
queue_size=10)
thr_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_attitude/att_throttle",
Float64,
queue_size=10)
Astar_grid_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_grid",
GridCells,
queue_size=10)
Astar_path_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_path",
GridCells,
queue_size=10)
arming_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/cmd/arming",
CommandBool)
set_mode_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/set_mode",
SetMode)
for i in range(4):
#velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i)
position_sub = rospy.Subscriber("/uav" + repr(i + 1) +
"/mavros/local_position/pose",
PoseStamped,
queue_size=10,
callback=position_cb,
callback_args=(i, int(uav_ID) - 1))
scan_lidar_sub = rospy.Subscriber("/scan",
LaserScan,
queue_size=10,
callback=scan_lidar_cb,
callback_args=uav_ID)
br = tf.TransformBroadcaster()
r = rospy.Rate(10)
print "TRY TO CONNECT"
while ((not rospy.is_shutdown()) and (not current_state.connected)):
#rospy.spinOnce()
r.sleep()
#print(current_state.connected.__class__)
rospy.loginfo("CURRENT STATE CONNECTED")
poses = PoseStamped()
#poses.pose = Pose()
#poses.pose.position = Point()
target = Pose()
if (idx == 0):
target.position.x = 0
target.position.y = 0
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 0
target.position.y = 0
if (idx == 3):
target.position.x = 4
target.position.y = 4
target.position.z = 10
poses.pose.position.x = target.position.x
poses.pose.position.y = target.position.y
poses.pose.position.z = target.position.z
q = quaternion_from_euler(0, 0, 45 * np.pi / 180.0)
poses.pose.orientation.x = q[0]
poses.pose.orientation.y = q[1]
poses.pose.orientation.z = q[2]
poses.pose.orientation.w = q[3]
i = 100
#while((not rospy.is_shutdown()) and (i>0)):
# local_pos_pub.publish(poses)
# i = i-1
rviz_visualization_start = False
last_request = rospy.Time.now()
count = 0
if (idx == 1):
target.position.x = 28
target.position.y = 28
if (idx == 2):
target.position.x = 0
target.position.y = 17
#thread1 = Thread(target = key_function)
#thread1.start()
style.use('fivethirtyeight')
thread2 = Thread(target=plot_error_live)
thread2.start()
thread3 = Thread(target=calculate_and_publish_wall,
args=(Astar_grid_pub, Astar_path_pub, idx))
thread3.start()
#file_error_pos_x = open(dir_path+'/txt/error_pos_x.txt','w')
#file_error_pos_y = open(dir_path+'/txt/error_pos_y.txt','w')
#file_error_pos_z = open(dir_path+'/txt/error_pos_z.txt','w')
error_time = 0
print poses
uav_color = [255, 0, 0, 255]
topic = 'uav_marker_' + uav_ID
uav_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_marker = Marker()
uav_marker.header.frame_id = "world"
uav_marker.type = uav_marker.SPHERE
uav_marker.action = uav_marker.ADD
uav_marker.scale.x = par.ws_model['robot_radius'] * 3
uav_marker.scale.y = par.ws_model['robot_radius'] * 3
uav_marker.scale.z = 0.005 * par.RealScale
uav_marker.color.r = float(uav_color[0]) / 255
uav_marker.color.g = float(uav_color[1]) / 255
uav_marker.color.b = float(uav_color[2]) / 255
uav_marker.color.a = float(uav_color[3]) / 255
uav_marker.pose.orientation.w = 1.0
uav_marker.pose.position.x = 0 #init_pos[0]
uav_marker.pose.position.y = 0 #init_pos[1]
uav_marker.pose.position.z = 0 #init_pos[2]
uav_marker_publisher.publish(uav_marker)
uav_color = [255, 255, 255, 255]
topic = 'uav_goal_marker_' + uav_ID
uav_goal_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_goal_marker = Marker()
uav_goal_marker.header.frame_id = "world"
uav_goal_marker.type = uav_goal_marker.SPHERE
uav_goal_marker.action = uav_goal_marker.ADD
uav_goal_marker.scale.x = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.y = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.z = 0.005 * par.RealScale
uav_goal_marker.color.r = float(uav_color[0]) / 255
uav_goal_marker.color.g = float(uav_color[1]) / 255
uav_goal_marker.color.b = float(uav_color[2]) / 255
uav_goal_marker.color.a = float(uav_color[3]) / 255
uav_goal_marker.pose.orientation.w = 1.0
uav_goal_marker.pose.position.x = 0 #init_pos[0]
uav_goal_marker.pose.position.y = 0 #init_pos[1]
uav_goal_marker.pose.position.z = 0 #init_pos[2]
uav_goal_marker_publisher.publish(uav_goal_marker)
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
last_request_rviz = rospy.Time.now()
last_HRVO_request = rospy.Time.now()
while (not rospy.is_shutdown()):
if (rviz_visualization_start and
(rospy.Time.now() - last_request_rviz > rospy.Duration(0.2))):
publish_uav_position_rviz(
br, list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, uav_ID)
uav_marker = update_uav_marker(
uav_marker, (list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, 1.0))
uav_marker_publisher.publish(uav_marker)
last_request_rviz = rospy.Time.now()
if ((not rviz_visualization_start)
and (current_state.mode != 'OFFBOARD')
and (rospy.Time.now() - last_request > rospy.Duration(1.0))):
offb_set_mode = set_mode_client(0, 'OFFBOARD')
if (offb_set_mode.mode_sent):
rospy.loginfo("OFFBOARD ENABLED")
last_request = rospy.Time.now()
else:
if ((not current_state.armed) and
(rospy.Time.now() - last_request > rospy.Duration(1.0))):
arm_cmd = arming_client(True)
if (arm_cmd.success):
rospy.loginfo("VEHICLE ARMED")
rviz_visualization_start = True
last_request = rospy.Time.now()
#print distance3D(cur_pose.pose,target)
if (distance3D(cur_pose.pose, target) <
0.5) and (not usingvelocitycontrol):
print "sampai"
usingvelocitycontrol = True
usingpositioncontrol = False
target = Pose()
if (idx == 0):
target.position.x = 28
target.position.y = 28
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 21
target.position.y = 14
if (idx == 3):
target.position.x = 0
target.position.y = 0
target.position.z = 10
print target
psi_target = 45 * np.pi / 180.0 #np.pi/180.0 #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
diagram4 = GridWithWeights(par.NumberOfPoints, par.NumberOfPoints)
diagram4.walls = []
for i in range(par.NumberOfPoints
): # berikan wall pada algoritma pathfinding A*
for j in range(par.NumberOfPoints):
if (par.ObstacleRegion[j, i] == 0):
diagram4.walls.append((i, j))
for i in range(par.NumberOfPoints):
for j in range(par.NumberOfPoints):
if (par.ObstacleRegionWithClearence[j][i] == 0):
diagram4.weights.update({(i, j): par.UniversalCost})
goal_pos = (target.position.x / par.RealScale,
target.position.y / par.RealScale)
start_pos = (cur_pose.pose.position.x / par.RealScale,
cur_pose.pose.position.y / par.RealScale)
pathx, pathy = Astar_version1(par, start_pos, goal_pos, diagram4)
print pathx
print pathy
vController.setHeadingTarget(deg2rad(90.0)) #45.0))
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
vController.setTarget(target)
vController.setPIDGainX(1, 0.0, 0.0)
vController.setPIDGainY(1, 0.0, 0.0)
vController.setPIDGainZ(1, 0, 0)
vController.setPIDGainPHI(1, 0.0, 0.0)
vController.setPIDGainTHETA(1, 0.0, 0.0)
vController.setPIDGainPSI(1, 0.0, 0.0)
if (usingpositioncontrol):
print "kontrol posisi"
poses.pose.position.x = 29
poses.pose.position.y = 29
poses.pose.position.z = 10
local_pos_pub.publish(poses)
elif (usingvelocitycontrol):
if (distance2D(cur_pose.pose, target) < 1.5):
if (len(pathx) > 1):
del pathx[0]
del pathy[0]
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker, (target.position.x, target.position.y,
target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
print target
vController.setTarget(target)
if (rospy.Time.now() - last_HRVO_request > rospy.Duration(0.05)):
last_HRVO_request = rospy.Time.now()
header.stamp = rospy.Time.now()
des_vel = vController.update(cur_pose)
current_agent_pose = (
list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z)
current_agent_vel = (list_current_velocity[idx].twist.linear.x,
list_current_velocity[idx].twist.linear.y)
current_neighbor_pose = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_pose.append(
(list_current_position[i].pose.position.x,
list_current_position[i].pose.position.y))
current_neighbor_vel = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_vel.append(
(list_current_velocity[i].twist.linear.x,
list_current_velocity[i].twist.linear.y))
V_des = (des_vel.twist.linear.x, des_vel.twist.linear.y)
quat_arr = np.array([
list_current_position[idx].pose.orientation.x,
list_current_position[idx].pose.orientation.y,
list_current_position[idx].pose.orientation.z,
list_current_position[idx].pose.orientation.w
])
att = euler_from_quaternion(quat_arr, 'sxyz')
#V = locplan.RVO_update_single(current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par,list_velocity_angle,list_distance_obs,att[2])
V, RVO_BA_all = locplan.RVO_update_single_static(
current_agent_pose, current_neighbor_pose,
current_agent_vel, current_neighbor_vel, V_des, par)
V_msg = des_vel
V_msg.twist.linear.x = V[0]
V_msg.twist.linear.y = V[1]
local_vel_pub.publish(V_msg)
goal = (target.position.x, target.position.y,
target.position.z)
x = current_position
list_error[0].append(error_time)
list_error[1].append(goal[0] - x[0])
list_error[2].append(goal[1] - x[1])
list_error[3].append(goal[2] - x[2])
error_time = error_time + 1
k = 0.1
vx = k * (goal[0] - x[0])
vy = k * (goal[1] - x[1])
vz = k * (goal[2] - x[2])
postar = PositionTarget()
postar.header = header
postar.coordinate_frame = PositionTarget().FRAME_LOCAL_NED
p = PositionTarget().IGNORE_PX | PositionTarget(
).IGNORE_PY | PositionTarget().IGNORE_PZ
a = PositionTarget().IGNORE_AFX | PositionTarget(
).IGNORE_AFY | PositionTarget().IGNORE_AFZ
v = PositionTarget().IGNORE_VX | PositionTarget(
).IGNORE_VY | PositionTarget().IGNORE_VZ
y = PositionTarget().IGNORE_YAW | PositionTarget(
).IGNORE_YAW_RATE
f = PositionTarget().FORCE
postar.type_mask = a | y | p | f #| PositionTarget().IGNORE_YAW | PositionTarget().IGNORE_YAW_RATE | PositionTarget().FORCE
postar.velocity.x = vx
postar.velocity.y = vy
postar.velocity.z = vz
postar.position.x = goal[0]
postar.position.y = goal[1]
postar.position.z = goal[2]
vel_msg = TwistStamped()
vel_msg.header = header
vel_msg.twist.linear.x = xvel
vel_msg.twist.linear.y = yvel
vel_msg.twist.linear.z = 0.0
vel_msg.twist.angular.x = 0.0
vel_msg.twist.angular.y = 0.0
vel_msg.twist.angular.z = 0.0
q = quaternion_from_euler(0, 0, 0.2)
att = PoseStamped()
att.pose.orientation.x = q[0]
att.pose.orientation.y = q[1]
att.pose.orientation.z = q[2]
att.pose.orientation.w = q[3]
att.pose.position.x = 0.0
att.pose.position.y = 0.0
att.pose.position.z = 2.0
cmd_thr = Float64()
cmd_thr.data = 0.3
att_raw = AttitudeTarget()
att_raw.header = Header()
att_raw.body_rate = Vector3()
att_raw.thrust = 0.7 #Float64()
att_raw.header.stamp = rospy.Time.now()
att_raw.header.frame_id = "fcu"
att_raw.type_mask = 71
att_raw.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0.2))
else:
local_pos_pub.publish(poses)
count = count + 1
r.sleep()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
def main():
# INITIALIZE ADAPTIVE NETWORK PARAMETERS:
N_INPUT = 3 # Number of input network
N_OUTPUT = 1 # Number of output network
INIT_NODE = 1 # Number of node(s) for initial structure
INIT_LAYER = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
N_WINDOW = 500 # Sliding Window Size
EVAL_WINDOW = 3 # Evaluation Window for layer Adaptation
DELTA = 0.05 # Confidence Level for layer Adaptation (0.05 = 95%)
ETA = 0.0001 # Minimum allowable value if divided by zero
FORGET_FACTOR = 0.98 # Forgetting Factor of Recursive Calculation
#EXP_DECAY = 1 # Learning Rate decay factor
#LEARNING_RATE = 0.00005 # Network optimization learning rate
LEARNING_RATE = 0.00002 / 2 # Network optimization learning rate
WEIGHT_DECAY = 0.000125 / 5 # Regularization weight decay factor
#WEIGHT_DECAY = 0.0 # Regularization weight decay factor
# INITIALIZE SYSTEM AND SIMULATION PARAMETERS:
IRIS_THRUST = 0.5629 # Nominal thrust for IRIS Quadrotor
RATE = 30.0 # Sampling Frequency (Hz)
# PID_GAIN_X = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-X
# PID_GAIN_Y = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-Y
PID_GAIN_X = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-X
PID_GAIN_Y = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-Y
#PID_GAIN_Z = [0.013,0.0004,0.2] # PID Gain Parameter [KP,KI,KD] axis-Z
PID_GAIN_Z = [0.013, 0.0, 0.2] # PID Gain Parameter [KP,KI,KD] axis-Z
SIM_TIME = 200 # Simulation time duration (s)
# Initial conditions of UAV system
xref = 0.0
yref = 0.0
zref = 2.0
interrx = 0.0
interry = 0.0
interrz = 0.0
errlastx = 0.0
errlasty = 0.0
errlastz = 0.0
runtime = 0.0
# Ignite the Evolving NN Controller
Xcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
Ycon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
Zcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
if INIT_LAYER > 2:
pdb.set_trace()
for i in range(INIT_LAYER - 2):
Xcon.addhiddenlayer()
Ycon.addhiddenlayer()
Zcon.addhiddenlayer()
dt = 1 / RATE
Xcon.dt = dt
Ycon.dt = dt
Zcon.dt = dt
Xcon.smcpar = PID_GAIN_X
Ycon.smcpar = PID_GAIN_Y
Zcon.smcpar = PID_GAIN_Z
# PX4 API object
modes = fcuModes() # Flight mode object
uav = uavCommand() # UAV command object
# Data Logger object
logData = dataLogger()
xconLog = dataLogger()
yconLog = dataLogger()
zconLog = dataLogger()
# Initiate node and subscriber
rospy.init_node('setpoint_node', anonymous=True)
rate = rospy.Rate(RATE) # ROS loop rate, [Hz]
# Subscribe to drone state
rospy.Subscriber('mavros/state', State, uav.stateCb)
# Subscribe to drone's local position
#rospy.Subscriber('mavros/local_position/pose', PoseStamped, uav.posCb)
rospy.Subscriber('mavros/mocap/pose', PoseStamped, uav.posCb)
# Setpoint publisher
att_sp_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
# Initiate Attitude messages
att = AttitudeTarget()
att.body_rate = Vector3()
att.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
att.thrust = IRIS_THRUST
att.type_mask = 7
# Arming the UAV --> no auto arming
text = colored("Ready for Arming..Press Enter to continue...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
while not uav.state.armed:
modes.setArm()
rate.sleep()
text = colored('Vehicle is arming..', 'yellow')
print(text)
# Switch to OFFBOARD after send few setpoint messages
text = colored("Vehicle Armed!! Press Enter to switch OFFBOARD...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
while not uav.state.armed:
modes.setArm()
rate.sleep()
text = colored('Vehicle is arming..', 'yellow')
print(text)
rate.sleep()
k = 0
while k < 10:
att_sp_pub.publish(att)
rate.sleep()
k = k + 1
modes.setOffboardMode()
text = colored('Now in OFFBOARD Mode..',
'blue',
attrs=['reverse', 'blink'])
print(text)
# ROS Main loop::
k = 0
while not rospy.is_shutdown():
start = time.time()
rate.sleep()
# Setpoint generator
if runtime <= 20:
xref = 0.0
yref = 0.0
zref = runtime * 1.5 / 20.0
if runtime > 20 and runtime <= 30:
xref = (runtime - 20) * 1.0 / 10.0
#xref = (runtime-20)* 5.0/10.0
yref = 0.0
zref = 1.5
if runtime > 30:
xref = 0.0 + 1.0 * math.cos(math.pi * (runtime - 30) / 20.0)
yref = 0.0 + 1.0 * math.sin(math.pi * (runtime - 30) / 20.0)
#xref = 0.0+5.0*math.cos(math.pi*(runtime-30)/60.0)
#yref = 0.0+5.0*math.sin(math.pi*(runtime-30)/60.0)
zref = 1.5
# Adjust the number of nodes in the latest layer (Network Width)
Xcon.adjustWidth(FORGET_FACTOR, N_WINDOW)
Ycon.adjustWidth(FORGET_FACTOR, N_WINDOW)
#Zcon.adjustWidth(FORGET_FACTOR,N_WINDOW)
# Adjust the number of layer (Network Depth)
Xcon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
Ycon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
#Zcon.adjustDepth(ETA,DELTA,N_WINDOW,EVAL_WINDOW)
# update current position
xpos = uav.local_pos.x
ypos = uav.local_pos.y
zpos = uav.local_pos.z
# calculate errors,delta errors, and integral errors
errx, derrx, interrx = Xcon.calculateError(xref, xpos)
erry, derry, interry = Ycon.calculateError(yref, ypos)
errz, derrz, interrz = Zcon.calculateError(zref, zpos)
# PID Controller equations
#theta_ref = 0.04*errx+0.0005*interrx+0.01*derrx
#phi_ref = 0.04*erry+0.0005*interry+0.01*derry
# vx = PID_GAIN_X[0]*errx+PID_GAIN_X[1]*interrx+PID_GAIN_X[2]*derrx
# vy = PID_GAIN_Y[0]*erry+PID_GAIN_Y[1]*interry+PID_GAIN_Y[2]*derry
#thrust_ref = IRIS_THRUST+PID_GAIN_Z[0]*errz+PID_GAIN_Z[1]*interrz+PID_GAIN_Z[2]*derrz
# SMC + NN controller
vx = Xcon.controlUpdate(xref) # Velocity X
vy = Ycon.controlUpdate(yref) # Velocity Y
thz = Zcon.controlUpdate(zref) # Additional Thrust Z
euler = euler_from_quaternion(uav.q)
psi = euler[2]
phi_ref, theta_ref = transform_control_signal(vx, vy, psi)
thrust_ref = IRIS_THRUST + thz
# control signal limiter
#phi_ref = limiter(-1*phi_ref,0.2)
phi_ref = limiter(-1 * phi_ref, 0.25)
theta_ref = limiter(-1 * theta_ref, 0.25)
att.thrust = limiter(thrust_ref, 1)
# Phi, Theta and Psi quaternion transformation
att.orientation = Quaternion(
*quaternion_from_euler(phi_ref, theta_ref, 0.0))
# Publish the control signal
att_sp_pub.publish(att)
# NN Learning stage
Xcon.optimize(LEARNING_RATE, WEIGHT_DECAY)
Ycon.optimize(LEARNING_RATE, WEIGHT_DECAY)
Zcon.optimize(LEARNING_RATE, WEIGHT_DECAY)
# Print all states
print('Xr,Yr,Zr = ', xref, yref, zref)
print('X, Y, Z = ', uav.local_pos.x, uav.local_pos.y,
uav.local_pos.z)
print('phi, theta = ', phi_ref, theta_ref)
print('att angles = ', euler)
print('Thrust = ', att.thrust)
print('Nodes X Y Z = ', Xcon.nNodes, Ycon.nNodes, Zcon.nNodes)
print('Layer X Y Z = ', Xcon.nLayer, Ycon.nLayer, Zcon.nLayer)
k += 1
runtime = k * dt
# Append Data Logger
logData.appendStateData(runtime, xref, yref, zref, xpos, ypos, zpos,
phi_ref, theta_ref, thrust_ref)
xconLog.appendControlData(runtime, Xcon.us, Xcon.un, Xcon.u,
Xcon.nNodes)
yconLog.appendControlData(runtime, Ycon.us, Ycon.un, Ycon.u,
Ycon.nNodes)
zconLog.appendControlData(runtime, Zcon.us, Zcon.un, Zcon.u,
Zcon.nNodes)
print('Runtime = ', runtime)
print('Exec time = ', time.time() - start)
print('')
# Break condition
if runtime > SIM_TIME:
modes.setRTLMode()
text = colored('Auto landing mode now..',
'blue',
attrs=['reverse', 'blink'])
print(text)
text = colored('Now saving all logged data..',
'yellow',
attrs=['reverse', 'blink'])
print(text)
logData.plotFigure()
xconLog.plotControlData()
yconLog.plotControlData()
zconLog.plotControlData()
text = colored('Mission Complete!!',
'green',
attrs=['reverse', 'blink'])
print(text)
break
def hover_rec(self, thrust, beh_type, time_flying):
if self.beh_type == 'HOVER' and time_flying <= 0:
print("Hovering time ended")
return self.landing_rec(target_raw_attitude.thrust, beh_type,
self.Max_time_landing)
elif beh_type == 'HOVER' and (self.hover_sensor_altitude_max >=
self.down_sensor_distance >=
self.hover_sensor_altitude_min):
print("The drone is hovering")
beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(
self.Time_between_messages) #was 0.005 (now 50hz ,500loops)
time_flying = time_flying - self.Time_between_messages
return self.hover_rec(target_raw_attitude.thrust, beh_type,
time_flying)
elif beh_type == 'HOVER' and (
self.hover_sensor_altitude_max <=
self.down_sensor_distance): # We have to go down
print("Recovering hover position - going down")
beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust - self.Deaccumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
time_flying = time_flying - self.Time_between_messages
return self.hover_rec(target_raw_attitude.thrust, beh_type,
time_flying)
elif beh_type == 'HOVER' and (
self.down_sensor_distance <=
self.hover_sensor_altitude_min): # We have to go up
print("Recovering hover position - going up")
beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust + self.Deaccumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
time_flying = time_flying - self.Time_between_messages
return self.hover_rec(target_raw_attitude.thrust, beh_type,
time_flying)
turn = turn * kb.speedBindings[key][1]
print(kb.vels(speed, turn))
if (status == 14):
print(kb.msg)
status = (status + 1) % 15
else:
#x = 0
#y = 0
#z = 0
#th = 0
if (key == '\x03'):
break
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = kb.imu.orientation
target_raw_attitude.body_rate.x = kb.acc_x # ROLL_RATE
target_raw_attitude.body_rate.y = kb.acc_y # PITCH_RATE
target_raw_attitude.body_rate.z = kb.acc_z # YAW_RATE
target_raw_attitude.thrust = kb.acc_thrust
attitude_target_pub.publish(target_raw_attitude)
#twist = Twist()
#twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed
#twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
#pub.publish(twist)
except Exception as e:
print(e)
finally:
from time import sleep
import rospy
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Vector3
from mavros_msgs.msg import Thrust
from mavros_msgs.msg import AttitudeTarget
node = rospy.init_node("thrust_test")
rate = rospy.Rate(20) # Hz
pub = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
print("looping!")
while True:
msg = AttitudeTarget()
msg.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
msg.body_rate = Vector3(x=0.0, y=0.0, z=0.0)
msg.thrust = -0.001
pub.publish(msg)
rate.sleep()
print("Sending thrust [%f]!" % msg.thrust)
