def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
if gps_qual == 0:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_qual == 1:
current_fix.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
elif gps_qual == 9:
# Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
# http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
hdop = data['hdop']
current_fix.position_covariance[0] = hdop**2
current_fix.position_covariance[4] = hdop**2
current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
def __init__(self):
rospy.init_node('lezl')
self.pose = PoseStamped()
self.state_sub = rospy.Subscriber('/mavros/state',State,self.state_cb)
self.odom_sub = rospy.Subscriber('/p3at/odom',Odometry,self.odom_cb)
self.tracked_sub = rospy.Subscriber('/tracker/tracked',Bool,self.track_cb)
self.landed_sub = rospy.Subscriber('/lezl/landed',Bool,self.landed_cb)
self.track_pub = rospy.Publisher('/tracker/tracked',Bool,queue_size=10)
self.tag_track_pub = rospy.Publisher('/tag_detections/tracked',Bool,queue_size=10)
self.state_pub = rospy.Publisher('/lezl/state',TwistStamped,queue_size=10)
self.heading_sub = rospy.Subscriber('/tracker/heading',TwistStamped,self.head_cb)
self.apriltag_sub = rospy.Subscriber('/tag_detections',AprilTagDetectionArray,self.parse_tags)
local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local',PoseStamped,queue_size=10)
self.cam_info_sub = rospy.Subscriber('/camera1/camera_info',CameraInfo,self.get_cam_info)
self.cam_info_pub = rospy.Publisher('/landing_pad/camera_info',CameraInfo,queue_size=10)
local_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped,queue_size=10)
self.arming_client = rospy.ServiceProxy('/mavros/cmd/arming',CommandBool)
self.set_mode_client = rospy.ServiceProxy('/mavros/set_mode',SetMode)
command_client = rospy.ServiceProxy('/mavros/cmd/command',CommandLong)
self.state = State()
self.tag_track = Bool()
self.cam_info = CameraInfo()
self.tracked = False
self.landed = False
self.lezl_state = TwistStamped()
rate = rospy.Rate(20)
while self.state.connected:
rospy.spinOnce()
rate.sleep()
self.pose.pose.position.x = 0
self.pose.pose.position.y = 0
self.pose.pose.position.z = 5
wait = rospy.Time.now()
offb = True
print "Starting Mission"
while not rospy.is_shutdown():
if self.landed:
print "Landed!!!"
break
# print (rospy.Time.now() - wait)>rospy.Duration(5)
if (self.state.mode != "OFFBOARD" and
(rospy.Time.now() - wait) > rospy.Duration(5)):
print "Attempting to enter OFFBOARD"
self.mode_switch("OFFBOARD")
start = rospy.Time.now()
else:
if (not self.state.armed and
(rospy.Time.now() - wait) > rospy.Duration(5)):
print "Attempting to arm"
self.arm_cmd(1)
if not offb and self.state.mode == 'OFFBOARD':
print "Watching for landing"
self.watchdog(start)
self.pose.header.stamp = rospy.Time.now()
if not self.tracked:
local_pos_pub.publish(self.pose)
else:
pass # Controller() class should be handling this
#local_vel_pub.publish(self.heading)
rate.sleep()
#!/usr/bin/env python
# Converts mavros/imu/mag from sensor_msgs/MagneticField to geometry_msgs/TwistStamped
# for displaying in rviz
# https://jsk-docs.readthedocs.io/en/latest/jsk_visualization/doc/jsk_rviz_plugins/index.html
import rospy
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import MagneticField
rospy.init_node('mag_to_vector')
vec = TwistStamped()
vec_pub = rospy.Publisher('mag_vec', TwistStamped, queue_size=1)
def mag_cb(msg):
vec.header = msg.header
vec.twist.linear = msg.magnetic_field
vec_pub.publish(vec)
rospy.Subscriber('mavros/imu/mag', MagneticField, mag_cb)
rospy.spin()
def add_sentence(self, nmea_string, frame_id, timestamp=None):
"""Public method to provide a new NMEA sentence to the driver.
Args:
nmea_string (str): NMEA sentence in string form.
frame_id (str): TF frame ID of the GPS receiver.
timestamp(rospy.Time, optional): Time the sentence was received.
If timestamp is not specified, the current time is used.
Returns:
bool: True if the NMEA string is successfully processed, False if there is an error.
"""
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
if not self.use_GNSS_time:
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with
# epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (2 * hdop *
self.alt_std_dev)**2 # FIXME
self.fix_pub.publish(current_fix)
# Publication of the latitude to the topic
self.latitude_pub.publish(float(latitude))
# Publication of the longitud to the topic
self.longitud_pub.publish(float(longitude))
# Publication of the height to the topic
self.height_pub.publish(float(altitude))
# Publication of the position_covariance_0 to the topic
self.position_covariance_0_pub.publish(
float(current_fix.position_covariance[0]))
# Publication of the position_covariance_4 to the topic
self.position_covariance_4_pub.publish(
float(current_fix.position_covariance[4]))
# Publication of the position_covariance_8 to the topic
self.position_covariance_8_pub.publish(
float(current_fix.position_covariance[8]))
# Publication of the position_covariance_type to the topic
self.position_covariance_type_pub.publish(
float(current_fix.position_covariance_type))
# Publication of the status to the topic
self.position_status_pub.publish(float(current_fix.status.status))
# Publication of the status service to the topic
self.position_status_service_pub.publish(
float(current_fix.status.service))
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * math.sin(
data['true_course'])
current_vel.twist.linear.y = data['speed'] * math.cos(
data['true_course'])
self.vel_pub.publish(current_vel)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
# Publication of the messages to the topics
self.latitude_pub.publish(float(latitude))
self.longitud_pub.publish(float(longitude))
# Publication of the position_covariance_0 to the topic
self.position_covariance_0_pub.publish(
float(current_fix.position_covariance[0]))
# Publication of the position_covariance_4 to the topic
self.position_covariance_4_pub.publish(
float(current_fix.position_covariance[4]))
# Publication of the position_covariance_8 to the topic
self.position_covariance_8_pub.publish(
float(current_fix.position_covariance[8]))
# Publication of the position_covariance_type to the topic
self.position_covariance_type_pub.publish(
float(current_fix.position_covariance_type))
# Publication of the status to the topic
self.position_status_pub.publish(
float(current_fix.status.status))
# Publication of the status service to the topic
self.position_status_service_pub.publish(
float(current_fix.status.service))
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide
# it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
else:
return False
avg_rewards = total_rewards[max(0, n - 100):(n + 1)].mean()
toc = time.time()
duration = str(toc - tic)
with summary_writer.as_default():
tf.summary.scalar('episode reward', total_reward, step=n)
tf.summary.scalar('running avg reward(100)', avg_rewards, step=n)
tf.summary.scalar('total volume', total_volume, step=n)
tf.summary.scalar('penetration_z', penetration_z, step=n)
tf.summary.scalar('episode duration', float(duration), step=n)
if n % 100 == 0:
print("episode:", n, "episode reward:", total_reward, "eps:",
epsilon, "avg reward (last 100):", avg_rewards)
print("episode reward:", total_reward)
print("avg reward for last 100 episodes:", avg_rewards)
env.close()
if __name__ == '__main__':
rospy.init_node('movingbobcat_gym', anonymous=True, log_level=rospy.WARN)
global depth, command, odom, pub2, volume, penetration_z
command = TwistStamped()
depth = 0
volume = 0.0
penetration_z = 0.0
rospy.Subscriber('/robot_bumper', ContactsState, get_depth)
rospy.Subscriber("/Bobby/odom", Odometry, odom_callback)
rospy.Subscriber('/volume', Float32, get_volume)
rospy.Subscriber('/penetration_z', Float32, get_penetration_z)
main()
from mavros_msgs.msg import State
from sensor_msgs.msg import BatteryState
import math
#Global constats
accuracy = 0.1
spd = 1.5
#Variaveis
goal_pose = PoseStamped()
current_state = State()
local = PoseStamped()
Drone_pose = PoseStamped()
Drone_speed = TwistStamped()
#State of drone
def state(state_data):
global current_state
current_state = state_data
#define the secure_accuracy to proced the mission
def secure_accuracy (x,y):
global accuracy
if (abs(x.pose.position.z - y.pose.position.z) < accuracy) and (abs(x.pose.position.y - y.pose.position.y) < accuracy) and (abs(x.pose.position.x - y.pose.position.x) < accuracy):
return True
return False
def __init__(self):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.IMPLEMENT_INVALID = -10000000.0
self.STATE_IDLE = 0
self.STATE_NAVIGATE = 1
self.STATE_WAIT = 2
self.state = self.STATE_IDLE
self.state_prev = self.state
self.automode_warn = False
self.wait_after_arrival = 0.0
self.wait_timeout = 0.0
self.status = 0
self.wpt = False
self.prev_wpt = False
self.linear_vel = 0.0
self.angular_vel = 0.0
self.pos = False
self.bearing = False
rospy.loginfo(rospy.get_name() + ": Start")
self.quaternion = np.empty((4, ), dtype=np.float64)
self.wii_a = False
self.wii_a_changed = False
self.wii_plus = False
self.wii_plus_changed = False
self.wii_minus = False
self.wii_minus_changed = False
self.wii_up = False
self.wii_up_changed = False
self.wii_down = False
self.wii_down_changed = False
self.wii_left = False
self.wii_left_changed = False
self.wii_right = False
self.wii_right_changed = False
self.wii_home = False
self.wii_home_changed = False
# get parameters
self.debug = rospy.get_param("~print_debug_information", 'true')
if self.debug:
rospy.loginfo(rospy.get_name() + ": Debug enabled")
self.status_publish_interval = rospy.get_param("~status_publish_interval", 0)
self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0)
# get topic names
self.automode_topic = rospy.get_param("~automode_sub",'/fmPlan/automode')
self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
self.joy_topic = rospy.get_param("~joy_sub",'/fmLib/joy')
self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
self.implement_topic = rospy.get_param("~implement_pub",'/fmCommand/implement')
self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmInformation/wptnav_status')
self.pid_topic = rospy.get_param("~pid_pub",'/fmInformation/wptnav_pid')
# setup publish topics
self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped, queue_size=1)
self.twist = TwistStamped()
self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped, queue_size=1)
self.implement = FloatStamped()
self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status, queue_size=5)
self.wptnav_status = waypoint_navigation_status()
self.status_publish_count = 0
self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped, queue_size=5)
self.pid = FloatArrayStamped()
self.pid_publish_count = 0
# configure waypoint navigation
self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
drive_kp = rospy.get_param("~drive_kp", 1.0)
drive_ki = rospy.get_param("~drive_ki", 0.0)
drive_kd = rospy.get_param("~drive_kd", 0.0)
drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
drive_max_output = rospy.get_param("~drive_max_output", 0.3)
turn_kp = rospy.get_param("~turn_kp", 1.0)
turn_ki = rospy.get_param("~turn_ki", 0.0)
turn_kd = rospy.get_param("~turn_kd", 0.2)
turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
turn_max_output = rospy.get_param("~turn_max_output", 0.5)
max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)
self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)
self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)
self.wpt_def_wait_after_arrival = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)
self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0)
target_ahead = rospy.get_param("~target_ahead", 1.0)
turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)
turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)
ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)
ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)
ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)
self.wptnav = waypoint_navigation(self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, self.wpt_def_tolerance, self.wpt_def_drive_vel, self.wpt_def_turn_vel, target_ahead, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist, self.debug)
self.wptlist = waypoint_list()
self.wptlist_loaded = False
# setup subscription topic callbacks
rospy.Subscriber(self.automode_topic, IntStamped, self.on_automode_message)
rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
# Init control methods
self.isNewGoalAvailable = self.empty_method()
self.isPreemptRequested = self.empty_method()
self.setPreempted = self.empty_method()
# Get topics and transforms
self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic",
"/fmSignals/cmd_vel")
self.odom_frame = rospy.get_param("~odom_frame", "/odom")
self.odometry_topic = rospy.get_param("~odometry_topic",
"/fmKnowledge/odom")
self.base_frame = rospy.get_param("~base_frame", "/base_footprint")
self.use_tf = rospy.get_param("~use_tf", False)
# Get general parameters
self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 1)
self.max_initial_error = rospy.get_param("~max_initial_angle_error", 1)
self.max_distance_error = rospy.get_param("~max_distance_error", 0.2)
self.use_tf = rospy.get_param("~use_tf", False)
self.max_angle_error = rospy.get_param("~max_angle_error", math.pi / 4)
self.retarder = rospy.get_param("~retarder", 0.8)
# Control loop
self.lin_p = rospy.get_param("~lin_p", 0.4)
self.lin_i = rospy.get_param("~lin_i", 0.6)
self.lin_d = rospy.get_param("~lin_d", 0.0)
self.ang_p = rospy.get_param("~ang_p", 0.8)
self.ang_i = rospy.get_param("~ang_i", 0.1)
self.ang_d = rospy.get_param("~ang_d", 0.05)
self.int_max = rospy.get_param("~int_max", 0.1)
# Setup Publishers and subscribers
if not self.use_tf:
self.odometry_topic = rospy.get_param("~odometry_topic",
"/fmKnowledge/odom")
self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry,
self.onOdometry)
self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
# Parameters for action server
self.period = 0.1
self.retarder_point = 0.3 #distance to target when speed should start declining
# Init control loop
self.lin_err = 0.0
self.ang_err = 0.0
self.lin_prev_err = 0.0
self.ang_prev_err = 0.0
self.lin_int = 0.0
self.ang_int = 0.0
self.lin_diff = 0.0
self.ang_diff = 0.0
self.distance_error = 0
self.angle_error = 0
self.fb_linear = 0.0
self.fb_angular = 0.0
self.sp_linear = 0.0
self.sp_angular = 0.0
# Init TF listener
self.__listen = TransformListener()
# Init controller
self.corrected = False
self.rate = rospy.Rate(1 / self.period)
self.twist = TwistStamped()
self.destination = Vector(0, 0)
self.position = Vector(0, 0)
self.heading = Vector(0, 0)
self.quaternion = np.empty((4, ), dtype=np.float64)
# Setup Publishers and subscribers
self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
def measSensorLoopTimerCallback(self, timer_msg):
# Get time
time_stamp_current = rospy.Time.now()
#
if (self.flag_robot_velocity_set == False):
return
# Computing the measurement
#
meas_vel_lin = np.zeros((3, ), dtype=float)
meas_vel_ang = np.zeros((3, ), dtype=float)
#
meas_vel_lin[0] = self.robot_vel_lin[0] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_vel_lin['x']))
meas_vel_lin[1] = self.robot_vel_lin[1] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_vel_lin['y']))
meas_vel_lin[2] = self.robot_vel_lin[2] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_vel_lin['z']))
#
meas_vel_ang[0] = self.robot_vel_ang[0] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_vel_ang['x']))
meas_vel_ang[1] = self.robot_vel_ang[1] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_vel_ang['y']))
meas_vel_ang[2] = self.robot_vel_ang[2] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_vel_ang['z']))
# Covariance
meas_cov_vel = np.diag([
self.cov_meas_vel_lin['x'], self.cov_meas_vel_lin['y'],
self.cov_meas_vel_lin['z'], self.cov_meas_vel_ang['x'],
self.cov_meas_vel_ang['y'], self.cov_meas_vel_ang['z']
])
# Filling the message
#
meas_header_msg = Header()
meas_robot_velocity_msg = Twist()
meas_robot_velocity_stamp_msg = TwistStamped()
meas_robot_velocity_stamp_cov_msg = TwistWithCovarianceStamped()
#
meas_header_msg.frame_id = self.robot_frame_id
meas_header_msg.stamp = self.robot_velocity_timestamp
# Linear
meas_robot_velocity_msg.linear.x = meas_vel_lin[0]
meas_robot_velocity_msg.linear.y = meas_vel_lin[1]
meas_robot_velocity_msg.linear.z = meas_vel_lin[2]
# Angular
meas_robot_velocity_msg.angular.x = meas_vel_ang[0]
meas_robot_velocity_msg.angular.y = meas_vel_ang[1]
meas_robot_velocity_msg.angular.z = meas_vel_ang[2]
#
meas_robot_velocity_stamp_msg.header = meas_header_msg
meas_robot_velocity_stamp_msg.twist = meas_robot_velocity_msg
#
meas_robot_velocity_stamp_cov_msg.header = meas_header_msg
meas_robot_velocity_stamp_cov_msg.twist.covariance = meas_cov_vel.reshape(
(36, 1))
meas_robot_velocity_stamp_cov_msg.twist.twist = meas_robot_velocity_msg
#
self.meas_robot_velocity_pub.publish(meas_robot_velocity_stamp_msg)
self.meas_robot_velocity_cov_pub.publish(
meas_robot_velocity_stamp_cov_msg)
#
return
class LandingController:
target = PoseStamped()
output = TwistStamped()
velocity = None
def __init__(self):
self.X = PID()
self.Y = PID()
self.Z = PID()
self.lastTime = rospy.get_time()
self.target = None
self.descentVelocity = -0.5
self.landingDescent = False
self.descending = False
self.distTolerance = 2
self.velTolerance = 0.1
# Send target to land at
def setTarget(self, target):
self.landingDescent = False
self.descending = False
self.target = target
def update(self, state, velocity):
self.velocity = velocity
if (self.target is None):
rospy.logwarn("Target position for landing controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
# create output structure
output = TwistStamped()
output.header = state.header
# check if we're at target altitude to initiate descent
distToTarget = math.pow(
position.x - self.target.position.x, 2) + math.pow(
position.y - self.target.position.y, 2) + math.pow(
position.z - self.target.position.z, 2)
if (not self.landingDescent and distToTarget < self.distTolerance):
self.landingDescent = True
print "Started landing descent"
# output velocities
linear = Vector3()
angular = Vector3()
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
if (self.landingDescent):
linear.z = self.descentVelocity
if (not self.descending and velocity.twist.linear.z <
(1 - self.velTolerance) * self.descentVelocity):
self.descending = True
print "Descending"
else:
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
# TODO
output.twist = Twist()
output.twist.linear = linear
return output
def landed(self):
return self.descending and (self.velocity.twist.linear.z > -0.2)
def stop(self):
setTarget(self.current)
update(self.current)
def get_desired_command(self):
if self._canceled:
return (TaskCanceled(), )
if self._state == TakeoffTaskState.init:
# Check if we have an fc status
if self._fc_status is None:
return (TaskRunning(), NopCommand())
# Check that auto pilot is enabled
if not self._fc_status.auto_pilot:
return (TaskFailed(
msg='flight controller not allowing auto pilot'), )
# Check that the FC is not already armed
if self._fc_status.armed:
return (TaskFailed(
msg='flight controller armed prior to takeoff'), )
# All is good change state to request arm
self._state = TakeoffTaskState.request_arm
# Enter the arming request stage
if self._state == TakeoffTaskState.request_arm:
# Check that the FC is not already armed
if self._arm_request_success:
self.pause_start_time = rospy.Time.now()
self._state = TakeoffTaskState.pause_before_takeoff
else:
return (TaskRunning(),
ArmCommand(True, True, False, self.arm_callback))
# Pause before ramping up the motors
if self._state == TakeoffTaskState.pause_before_takeoff:
if rospy.Time.now() - self.pause_start_time > rospy.Duration(
self._DELAY_BEFORE_TAKEOFF):
self._state = TakeoffTaskState.takeoff
return (TaskRunning(),
GroundInteractionCommand('takeoff',
self.takeoff_callback))
else:
return (TaskRunning(), NopCommand())
# Enter the takeoff phase
if self._state == TakeoffTaskState.takeoff:
return (TaskRunning(), )
if (self._state == TakeoffTaskState.ascend
or self._state == TakeoffTaskState.ascend_angle):
try:
transStamped = self._tf_buffer.lookup_transform(
'map', 'base_footprint', rospy.Time.now(),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
msg = 'Exception when looking up transform during takeoff'
rospy.logerr('Takeofftask: {}'.format(msg))
rospy.logerr(ex.message)
return (TaskAborted(msg=msg), )
# Check if we reached the target height
if (transStamped.transform.translation.z >
self._TAKEOFF_COMPLETE_HEIGHT):
self._state = TakeoffTaskState.done
return (TaskDone(), NopCommand())
elif (self._state == TakeoffTaskState.ascend
and transStamped.transform.translation.z >
self._ANGLE_MODE_HEIGHT):
self._state = TakeoffTaskState.ascend_angle
return (TaskRunning(),
ArmCommand(True, True, True, lambda _: None))
else:
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.z = self._TAKEOFF_VELOCITY
return (TaskRunning(), VelocityCommand(velocity))
if self._state == TakeoffTaskState.done:
return (TaskDone(), NopCommand())
if self._state == TakeoffTaskState.failed:
return (TaskFailed(msg='Take off task experienced failure'), )
# Impossible state reached
return (TaskAborted(msg='Impossible state in takeoff task reached'), )
def algo():
velocityPub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
headingSub = rospy.Subscriber('/mavros/global_position/compass_hdg',
Float64, heading)
rospy.Subscriber("/mavros/global_position/global", NavSatFix, getLoc)
rate = rospy.Rate(1) #1 H
wayList = readWaypoints()
global finalLong
global finalLat
global number
number = 0
r = rospy.Rate(1)
location()
# rospy.spin()
while not rospy.is_shutdown():
# print 'hii', float(finalLat)
# print type(float(finalLong))
finalLat = float(wayList[number][0])
finalLong = float(wayList[number][1])
pointA = (float(currLat), float(currLong))
# pointD = (float(28.544689), float(77.273489))
pointD = (float(finalLat), float(finalLong))
# des = calculate_initial_compass_bearing(pointA,pointD)
Wi1 = utm.from_latlon(currLat, currLat)
Wi2 = utm.from_latlon(finalLat, finalLong)
# des = atan2(Wi2[1] - Wi1[1], Wi2[0] - Wi1[0])
vmag, des = pot_fields(finalLat, finalLong)
print 'Des', des
print 'heading angle', currAng
error = calculateError(des, currAng)
print 'error', error
move = TwistStamped()
check = euclideanDisGPS(pointD, pointA)
print "distance from final waypoint:", check
if (check < 2): # if in the vicinity, then stop giving the control
move.twist.linear.x = 0
move.twist.linear.y = 0
move.twist.angular.z = 0
velocityPub.publish(move)
if (number < (len(wayList) - 1)):
number += 1
print 'len', len(wayList)
print 'number', number
else:
move.twist.linear.x = 3
move.twist.linear.y = 3
move.twist.angular.z = error * kp
velocityPub.publish(move)
# x = input()
r.sleep()
rospy.spin()
PROTOCOL = IoTHubTransportProvider.MQTT
# String containing Hostname, Device Id & Device Key in the format:
CONNECTION_STRING = "HostName=ez10S1.azure-devices.net;DeviceId=PC0;SharedAccessKey=uC98sguRyI1ytjvMxDHqdVjHG2F2rOCdcLNyREjWfts="
MSG_TXT = "{\"deviceId\": \"PC0\",\"datetime\": \"%s\",\"x\": %.8f,\"y\": %.8f,\"z\": %.8f,\"lati\": %.8f,\"longi\": %.8f,\"velx\": %.8f,\"vely\": %.8f,\"temp\": %.4f,\"humid\": %.4f}" ##JSON Type!?
# some embedded platforms need certificate information
# global variables, saving sensor data
# === IMU ===
imuMsg = Imu()
#rpyMsg = DiagnosticArray()
# === GPS ===
gpsMsg = NavSatFix()
velMsg = TwistStamped()
# === TEMP & HUMID ===
tempMsg = Float32()
humidMsg = Float32()
def set_certificates(client):
from iothub_client_cert import CERTIFICATES
try:
client.set_option("TrustedCerts", CERTIFICATES)
print("set_option TrustedCerts successful")
except IoTHubClientError as iothub_client_error:
print("set_option TrustedCerts failed (%s)" % iothub_client_error)
def receive_message_callback(message, counter):
def __init__(self):
self.force = 0
self.laser = 0
self.image = None
self.cX = 0
self.cY = 0
self.bridge = cv_bridge.CvBridge()
cv2.namedWindow("window", 1)
self.imu = Imu()
self.states = ManipulatorJoints()
self.forces = TwistStamped()
self.theta = [
0, math.pi / 15, math.pi / 15, -math.pi / 7.5, -math.pi / 2, 0
]
self.orientacao = [0.0, 0.0, 0.0]
self.pub_manipulator = rospy.Publisher('/ur5/jointsPosTargetCommand',
ManipulatorJoints,
queue_size=1)
self.sub_manipulatorStates = rospy.Subscriber(
'/ur5/jointsPositionCurrentState', ManipulatorJoints,
self.callback_Mstates)
#self.sub_manipulatorForces = rospy.Subscriber('/ur5/forceTorqueSensorOutput', TwistStamped, self.callback_Mforces)
self.sub_imu = rospy.Subscriber('/sensor/imu', Imu, self.callback_Imu)
self.sub_image = rospy.Subscriber('sensor/ur5toolCam', Image,
self.callback_Image)
self.sub_laser = rospy.Subscriber('sensor/hokuyo', HokuyoReading,
self.callback_Laser)
self.sub_force = rospy.Subscriber('ur5/forceTorqueSensorOutput',
TwistStamped, self.callback_Force)
# defining the eternal loop frequency
node_sleep_rate = rospy.Rate(10)
# eternal loop (until second order)
while not rospy.is_shutdown():
#manipulator = ManipulatorJoints()
orientation_q = self.imu.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z,
orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
self.orientacao = [roll, pitch, yaw]
angulos = []
distX = 320 - self.cX
kp = -0.0005
#for i in range(0, 6):
# s = "Escolha o angulos theta" + str(i) + "\n"
# ang = float(input(s))
# angulos.append(ang) # adding the element
#print(self.laser)
#self.theta = [0,0,0,0,0,0]
#self.theta[0] = kp*distX - math.pi/2 - yaw
print(self.force)
if self.force < 0.15:
self.theta[0] = -math.pi / 2 - yaw
if self.states < 1.2:
self.theta[1] += 0.004
self.theta[2] -= 0.007
self.theta[3] += 0.003
else:
self.theta[2] += 0.003
self.theta[3] -= 0.003
#self.theta[4] = -math.pi/2
#self.theta[5] = 0
#angulos = theta
#size = self.image.shape
#print(size)
#print(angulos)
angulos = self.theta
self.pub_manipulator.publish(joint_variable=angulos)
#theta = np.array(self.theta)
#theta = angulos
#self.theta = np.array(theta).tolist()
node_sleep_rate.sleep()
def send_cmd(self, linear, angular):
ts = TwistStamped()
ts.header.stamp = rospy.Time.now()
ts.twist.linear.x, ts.twist.linear.y, ts.twist.linear.z = linear
ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z = angular
self._pub.publish(ts)
def __init__(self, this_uav, NUM_UAV, D_GAIN):
print 'init'
print 'this uav = ' + str(this_uav)
print 'num uav = ' + str(NUM_UAV)
global cur_pose
global cur_state
global cur_vel
global cur_globalPose
cur_pose = [PoseStamped() for i in range(NUM_UAV)]
cur_globalPose = [Odometry() for i in range(NUM_UAV)]
cur_state = [State() for i in range(NUM_UAV)]
cur_vel = [TwistStamped() for i in range(NUM_UAV)]
des_pose = PoseStamped()
des_vel = TwistStamped()
des_vel.header.frame_id = "map"
rospy.init_node('offboard_test'+str(this_uav), anonymous=True)
rate = rospy.Rate(100) #Hz
mode_proxy = rospy.ServiceProxy('mavros'+str(this_uav + 1)+'/set_mode' ,SetMode)
arm_proxy = rospy.ServiceProxy('mavros'+str(this_uav + 1)+'/cmd/arming', CommandBool)
paramPull_proxy = rospy.ServiceProxy('mavros'+str(this_uav+1)+'/param/pull', ParamPull)
paramGet_proxy = rospy.ServiceProxy('mavros'+str(this_uav+1)+'/param/get', ParamGet)
paramSet_proxy = rospy.ServiceProxy('mavros'+str(this_uav+1)+'/param/set', ParamSet)
print('paramPull - \n' + str(paramPull_proxy(True)))
#print('rospy_getParam-COM_ARM_AUTH\n'+ str(rospy.get_param('/mavros'+str(this_uav+1)+'/param/COM_ARM_AUTH')))
print('paramGet MAV_TYPE - \n' + str(paramGet_proxy("MAV_TYPE")))
print('______________________________________________________________________________')
#generating subscribers to each drone
for i in range(NUM_UAV):
exec('def position_cb'+ str(i) +'(msg): cur_pose['+ str(i) +'] = msg')
exec('def globalPosition_cb'+str(i)+'(msg): cur_globalPose['+ str(i) +'] = msg')
exec('def velocity_cb'+ str(i) +'(msg): cur_vel['+ str(i) +'] = msg')
exec('def state_cb'+ str(i) +'(msg): cur_state['+ str(i) +'] = msg')
rospy.Subscriber('/mavros'+ str(i + 1) + '/local_position/pose', PoseStamped, callback= eval('position_cb'+ str(i)))
rospy.Subscriber('/mavros'+ str(i + 1) + '/global_position/local', TwistStamped, callback= eval('globalPosition_cb'+str(i)))
rospy.Subscriber('/mavros'+ str(i + 1) + '/state', State, callback= eval('state_cb'+str(i)))
rospy.Subscriber('/mavros'+ str(i + 1) + '/local_position/velocity', TwistStamped, callback=eval('velocity_cb'+ str(i)))
#suscribe to sequencer
rospy.Subscriber('/sequencer/command', Float64MultiArray, callback = self.command_cb)
#publish status to sequencer
status_pub = rospy.Publisher('/sequencer/status'+str(this_uav), Int8, queue_size = 10)
#publish position to mav
pose_pub = rospy.Publisher('/mavros'+ str(this_uav + 1) + '/setpoint_position/local', PoseStamped, queue_size = 10)
#publish velocity to mav
vel_pub = rospy.Publisher('/mavros'+str(this_uav + 1) + '/setpoint_velocity/cmd_vel', TwistStamped, queue_size = 10)
#GOTO initial holding pattern command = [0,0,25,0]
des_pose = cur_pose[this_uav]
des_pose.pose.position.x = 10 * math.sin((this_uav * 2 * math.pi) / NUM_UAV)
des_pose.pose.position.y = 10 * math.cos((this_uav * 2 * math.pi) / NUM_UAV)
des_pose.pose.position.z = 20
pose_pub.publish(des_pose)
status_pub.publish(self.status)
#....fix....double subscribe to status, check and set status in callback?
while cur_state[this_uav].mode != 'OFFBOARD' and not cur_state[this_uav].armed:
mode_sent = False
success = False
pose_pub.publish(des_pose)
while not mode_sent:
rospy.wait_for_service('mavros'+str(this_uav + 1)+'/set_mode', timeout = None)
try:
mode_sent = mode_proxy(1,'OFFBOARD')
except rospy.ServiceException as exc:
print exc
rate.sleep()
while not success:
rospy.wait_for_service('mavros'+str(this_uav + 1)+'/cmd/arming', timeout = None)
try:
success = arm_proxy(True)
except rospy.ServiceException as exc:
print exc
rate.sleep()
print 'mode_sent - ' + str(mode_sent)
print 'arm_sent - ' + str(success)
print 'armed?'
rate.sleep()
print cur_state[this_uav].armed
print cur_state[this_uav].mode
#wait for sequencer to connect to /sequencer/status#
nc = status_pub.get_num_connections()
print 'num_connections = ' +str(nc)
sys.stdout.flush()
while nc == 0:
nc = status_pub.get_num_connections()
rate.sleep()
print 'num_connections = ' + str(nc)
sys.stdout.flush()
rate.sleep()
#MAIN LOOP
print 'Loop INIT Time - ' + str(time.clock())
while not rospy.is_shutdown():
#pose_pub.publish(des_pose)
self.convergence = math.sqrt(math.pow(des_pose.pose.position.x-cur_pose[this_uav].pose.position.x,2)+math.pow(des_pose.pose.position.y-cur_pose[this_uav].pose.position.y,2)+math.pow(des_pose.pose.position.z-cur_pose[this_uav].pose.position.z,2))
if self.convergence < .5 and self.status != Int8(self.command.data[3]):
self.status = Int8(self.command.data[3])
print 'Status Set - '+ str(self.status) + ' time - '+ str(time.clock())
print 'Current Pose - ' + str(cur_pose[this_uav].pose)
status_pub.publish(self.status)
if self.command.data[3] > -1 and self.command.data[3] < 4:
des_pose.pose.position.x = self.command.data[0] + self.command.data[2]*math.sin((this_uav * 2 * math.pi)/NUM_UAV)
des_pose.pose.position.y = self.command.data[1] + self.command.data[2]*math.cos((this_uav * 2 * math.pi)/NUM_UAV)
des_pose.pose.position.z = 25
pose_pub.publish(des_pose)
if self.command.data[3] > 3:
des_vel.twist.linear.x = (this_uav - cur_globalPose[this_uav].pose.pose.position.x)
des_vel.twist.linear.y = (this_uav - cur_globalPose[this_uav].pose.pose.position.y)
des_vel.twist.linear.z = 0
vel_pub.publish(des_vel)
rate.sleep()
def __init__(self):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.automode = False
self.automode_prev = False
self.status = 0
self.wpt = False
self.prev_wpt = False
self.linear_speed = 0.0
self.angular_speed = 0.0
self.pos = False
self.bearing = False
rospy.loginfo(rospy.get_name() + ": Start")
self.quaternion = np.empty((4, ), dtype=np.float64)
self.wii_a = False
self.wii_a_changed = False
self.wii_plus = False
self.wii_plus_changed = False
self.wii_minus = False
self.wii_minus_changed = False
self.wii_up = False
self.wii_up_changed = False
self.wii_down = False
self.wii_down_changed = False
self.wii_left = False
self.wii_left_changed = False
self.wii_right = False
self.wii_right_changed = False
self.wii_home = False
self.wii_home_changed = False
# get parameters
self.debug = rospy.get_param("~print_debug_information", 'true')
if self.debug:
rospy.loginfo(rospy.get_name() + ": Debug enabled")
self.status_publish_interval = rospy.get_param(
"~status_publish_interval", 0)
# get topic names
self.automode_topic = rospy.get_param("~automode_sub",
'/fmDecisionMakers/automode')
self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
self.joy_topic = rospy.get_param("~joy_sub", '/fmLib/joy')
self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",
'/fmCommand/cmd_vel')
self.wptnav_status_topic = rospy.get_param("~status_pub",
'/fmData/wptnav_status')
# setup publish topics
self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
self.twist = TwistStamped()
self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic,
waypoint_navigation_status)
self.wptnav_status = waypoint_navigation_status()
self.status_publish_count = 0
# configure waypoint navigation
drive_kp = rospy.get_param("~drive_kp", 1.0)
drive_ki = rospy.get_param("~drive_ki", 0.0)
drive_kd = rospy.get_param("~drive_kd", 0.0)
drive_integral_max = rospy.get_param("~drive_integral_max", 1.0)
turn_kp = rospy.get_param("~turn_kp", 1.0)
turn_ki = rospy.get_param("~turn_ki", 0.0)
turn_kd = rospy.get_param("~turn_kd", 0.0)
turn_integral_max = rospy.get_param("~turn_integral_max", 1.0)
max_linear_velocity = rospy.get_param("~max_linear_velocity", 0.4)
max_angular_velocity = rospy.get_param("~max_angular_velocity", 0.4)
self.wpt_tolerance = rospy.get_param("~wpt_tolerance", 0.5)
wpt_target_distance = rospy.get_param("~wpt_target_distance", 1.0)
wpt_turn_start_at_heading_err = rospy.get_param(
"~wpt_turn_start_at_heading_err", 20.0)
wpt_turn_stop_at_heading_err = rospy.get_param(
"~wpt_turn_stop_at_heading_err", 1.0)
self.wpt_linear_velocity = rospy.get_param("~wpt_linear_velocity", 0.5)
wpt_ramp_down_velocity_at_distance = rospy.get_param(
"~wpt_ramp_down_velocity_at_distance", 1.0)
wpt_ramp_down_minimum_velocity = rospy.get_param(
"~wpt_ramp_down_minimum_velocity", 0.3)
self.wptnav = waypoint_navigation(
self.update_rate, drive_kp, drive_ki, drive_kd, drive_integral_max,
turn_kp, turn_ki, turn_kd, turn_integral_max, max_linear_velocity,
max_angular_velocity, self.wpt_tolerance, wpt_target_distance,
wpt_turn_start_at_heading_err, wpt_turn_stop_at_heading_err,
self.wpt_linear_velocity, wpt_ramp_down_velocity_at_distance,
wpt_ramp_down_minimum_velocity, self.debug)
self.wptlist = waypoint_list()
self.wptlist_loaded = False
# setup subscription topic callbacks
rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
self.imu = None
self.gps = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.takeoff_height = 15
self.local_enu_position = None
self.cur_target_pose = PoseStamped()
self.cur_target_twist = TwistStamped()
self.global_target = None
self.received_new_task = False
self.arm_state = False
self.offboard_state = False
self.received_imu = False
self.frame = "BODY"
self.flag = 0
self.state = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber(
"/uav2/mavros/local_position/pose", PoseStamped,
self.local_pose_callback)
self.mavros_sub = rospy.Subscriber("/uav2/mavros/state", State,
self.mavros_state_callback)
self.gps_sub = rospy.Subscriber("/uav2/mavros/global_position/global",
NavSatFix, self.gps_callback)
self.imu_sub = rospy.Subscriber("/uav2/mavros/imu/data", Imu,
self.imu_callback)
self.set_target_position_sub = rospy.Subscriber(
"/uav2/gi/set_pose/position", PoseStamped,
self.set_target_position_callback)
self.set_target_velocity_sub = rospy.Subscriber(
"/uav2/gi/set_pose/velocity", Vector3Stamped,
self.set_target_velocity_callback)
self.set_target_yaw_sub = rospy.Subscriber(
"/uav2/gi/set_pose/orientation", Float32,
self.set_target_yaw_callback)
self.custom_activity_sub = rospy.Subscriber(
"/uav2/gi/set_activity/type", String,
self.custom_activity_callback)
'''
ros publishers
'''
self.pose_target_pub = rospy.Publisher(
'/uav2/mavros/setpoint_position/local', PoseStamped, queue_size=10)
self.twist_target_pub = rospy.Publisher(
'/uav2/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy('/uav2/mavros/cmd/arming',
CommandBool)
self.flightModeService = rospy.ServiceProxy('/uav2/mavros/set_mode',
SetMode)
print("Px4 Controller Initialized!")
def __init__(self):
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.local_position = PoseStamped()
self.state = State()
self.mav_type = None
self.sub_topics_ready = {
key: False
for key in
['alt', 'ext_state', 'global_pos', 'local_pos', 'state']
}
# ROS services
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
self.fail("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
# Publisher
self.desired_pos = PoseStamped()
self.desired_pos.pose.position.x = 0
self.desired_pos.pose.position.y = 0
self.desired_pos.pose.position.z = 0
self.radius = 0.3
self.yaw_th = 0.05
self.z_in = 0.1
self.z_ub = 2.7 # Height upper bound
self.z_lb = 0.5 # Height lower bound
self.yaw_test = 0
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
#self.pos_thread = Thread(target=self.send_pos, args=())
#self.pos_thread.daemon = True
#self.pos_thread.start()
# ---------------- -------------------#
# Publishers
self.desired_vel = TwistStamped()
self.desired_vel.twist.angular.z = 0
self.desired_vel.twist.linear.x = 0
self.desired_vel.twist.linear.y = 0
self.desired_vel.twist.linear.z = 0
#self.radius = 0.3
#self.yaw_th = 0.05
self.yaw_current = 0
self.isLand = False
self.yawrate = 0.5
self.xvel = 0.5
self.x_target = 0
self.y_target = 0
self.z_target = 0
self.p_gain = 1.0
self.yaw_p_gain = 0.5
self.cmd_vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=1)
self.vel_thread = Thread(target=self.send_vel_from_pos, args=())
self.vel_thread.daemon = True
self.vel_thread.start()
def callback(self, twist_in):
twist_stamped = TwistStamped()
twist_stamped.header.stamp = rospy.Time.now()
twist_stamped.twist = twist_in
self._twist_stamped_pub.publish(twist_stamped)
t1 = +1.0 - 2.0 * (x * x + ysqr)
X = mt.degrees(mt.atan2(t0, t1))
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
Y = mt.degrees(mt.asin(t2))
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (ysqr + z * z)
Z = mt.degrees(mt.atan2(t3, t4))
return X, Y, Z
local_velocity = TwistStamped()
def lv_cb(data):
global local_velocity
local_velocity = data
local_position = PoseStamped()
def lp_cb(data):
global local_position
local_position = data
def on_shutdown(self):
rospy.loginfo("[%s] Shutting down." % (self.node_name))
if __name__ == '__main__':
# Initialize the node with rospy
rospy.init_node('rmp_base', anonymous=False)
#alive=threading.active_count()
#rospy.loginfo("alive thread num: %d"%alive) //when init the node alive thread is 4
#setup communication thread
rsp_queue = multiprocessing.Queue()
cmd_queue = multiprocessing.Queue()
in_flags = multiprocessing.Queue()
out_flags = multiprocessing.Queue()
vel = TwistStamped()
my_thread = threading.Thread(target=RMP,
args=(rmp_addr, rsp_queue, cmd_queue,
in_flags, out_flags, UPDATE_DELAY_SEC,
LOG_DATA))
my_thread.setDaemon(True)
my_thread.start()
#svreate event handler and set to BALANCE MODE
EventHandler = RMPEventHandlers(cmd_queue, rsp_queue, in_flags)
#EventHandler.GotoTractor()
#go to BALANCE
EventHandler.GotoBalance()
rospy.loginfo("finish initialize!")
# move along x
move = stages.MoveRelative("x +0.2", cartesian)
move.group = group
header = Header(frame_id = "world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2,0,0)))
task.add(move)
# move along y
move = stages.MoveRelative("y -0.3", cartesian)
move.group = group
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0,-0.3,0)))
task.add(move)
# rotate about z
move = stages.MoveRelative("z +90°", cartesian)
move.group = group
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0,0,pi/4.))))
task.add(move)
# moveTo named posture
move = stages.MoveTo("moveTo ready", cartesian)
move.group = group
move.setGoal("ready")
task.add(move)
if task.plan():
task.publish(task.solutions[0])
time.sleep(100)
print(vels(speed, turn))
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
continue
twist = Twist()
twist_st = TwistStamped()
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
twist_st.header.stamp = rospy.Time.now()
twist_st.twist = twist
pub.publish(twist_st)
except Exception as e:
print(e)
finally:
twist = Twist()
weights_3 = np.load(weights_dir + 'weights_3.npy').transpose()
bias_3 = np.load(weights_dir + 'bias_3.npy').transpose()
mean = np.load(output_dir + 'mean.npy')
std = np.load(output_dir + 'std.npy')
mean_x = np.load(output_dir_x + 'mean.npy')
std_x = np.load(output_dir_x + 'std.npy')
# print('mean', mean)
# print('std', std)
# exit()
target_flag_pub = False
vrpn = TwistStamped()
def vrpn_cb(data):
global vrpn
vrpn = data
target_pose = MultiDOFJointTrajectory()
def target_cb(data):
global target_pose
target_pose = data
global target_flag_pub
target_flag_pub = True
def brahms_process(persist, input):
# nominal output
output = {'event': {'response': S_NULL}}
# switch on event type
if input['event']['type'] == EVENT_MODULE_INIT:
# provide component information
output['info'] = {}
output['info']['component'] = (0, 1)
output['info']['additional'] = ('Author=David Buxton\n')
output['info']['flags'] = (F_NOT_RATE_CHANGER)
# ok
output['event']['response'] = C_OK
# switch on event type
elif input['event']['type'] == EVENT_STATE_SET:
# ok
output['event']['response'] = C_OK
# switch on event type
elif input['event']['type'] == EVENT_INIT_CONNECT:
# on first call
if input['event']['flags'] & F_FIRST_CALL:
# Access input port 'python_ros_in'
index = input['iif']['default']['index']['python_ros_in']
port = input['iif']['default']['ports'][index]
# Set robot name
topic_root = "/" + os.getenv("MIRO_ROBOT_NAME")
# Initialise ROS nodes
persist['ros_pub'] = rospy.Publisher(topic_root +
"/control/cmd_vel",
TwistStamped,
queue_size=10)
rospy.init_node('output', anonymous=True)
# Store data
persist['python_ros_in'] = port['data']
persist['velocity'] = TwistStamped()
# do nothing
pass
# on last call
if input['event']['flags'] & F_LAST_CALL:
# do nothing
pass
# ok
output['event']['response'] = C_OK
# switch on event type
elif input['event']['type'] == EVENT_RUN_SERVICE:
# Set wheel speeds as input data
denom = 100000000
wheel_speed = [
persist['python_ros_in'] / denom, persist['python_ros_in'] / denom
]
# Convert to command velocity
(dr, dtheta) = miro.utils.wheel_speed2cmd_vel(wheel_speed)
# Set velocity values
persist['velocity'].twist.linear.x = dr
persist['velocity'].twist.angular.z = dtheta
# Publish data to ROS node
persist['ros_pub'].publish(persist['velocity'])
# ok
output['event']['response'] = C_OK
# return
return (persist, output)
def toNED(msg):
# fliter measured velocity
global vx_arr, vy_arr
vx_arr.pop(0)
vx_arr.append(msg.velocity_x)
vy_arr.pop(0)
vy_arr.append(msg.velocity_y)
vx = 0.5 * vx_arr[4] + 0.2 * vx_arr[3] + 0.15 * vx_arr[2] + 0.1 * vx_arr[
1] + 0.05 * vx_arr[0]
vy = 0.5 * vy_arr[4] + 0.2 * vy_arr[3] + 0.15 * vy_arr[2] + 0.1 * vy_arr[
1] + 0.05 * vy_arr[0]
v_body = array([vx, -vy, 0]) * 1.2
# transform body velocity to ENU
global q
[qx, qy, qz, qw] = [q[0], q[1], q[2], q[3]]
Tenu = array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw
],
[
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz,
2 * qy * qz - 2 * qx * qw
],
[
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx * qx - 2 * qy * qy
]])
v = dot(Tenu, v_body)
# estimate z velocity by height derivative
global z_now, t_now, z_prev, t_prev
z_now = msg.ground_distance
t_now = msg.header.stamp.to_sec()
if ((t_now - t_prev) < 1):
vz = (z_now - z_prev) / (t_now - t_prev)
else:
vz = 0
# ENU to NED: (x,y,z) -> (x,-y,-z)
twist = TwistStamped()
twist.header = Header()
twist.header.frame_id = "ned"
twist.header.stamp = msg.header.stamp
twist.twist.linear.x = v[0]
twist.twist.linear.y = -v[1]
twist.twist.linear.z = -vz
pub.publish(twist)
z_prev = z_now
t_prev = t_now
# record data in vicon frame, compare with vicon
q_ned_vicon = tf.transformations.quaternion_from_euler(math.pi, 0, -yaw)
[qx, qy, qz,
qw] = [q_ned_vicon[0], q_ned_vicon[1], q_ned_vicon[2], q_ned_vicon[3]]
Tv = array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw
],
[
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz,
2 * qy * qz - 2 * qx * qw
],
[
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx * qx - 2 * qy * qy
]])
vr = dot(Tv, array([v[0], -v[1], 0]))
outtxt.write(str.format("{0:.9f} ", msg.header.stamp.to_sec()))
outtxt.write(str.format("{0:.9f} ", vr[0]))
outtxt.write(str.format("{0:.9f} ", vr[1]))
outtxt.write(str.format("{0:.9f} ", vz))
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0\n')
def main():
# INITIALIZE ADAPTIVE NETWORK PARAMETERS:
N_INPUT = 4 # 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)
INIT_NODE_X = 1 # Number of node(s) for initial structure
INIT_LAYER_X = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
INIT_NODE_Y = 1 # Number of node(s) for initial structure
INIT_LAYER_Y = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
INIT_NODE_Z = 1 # Number of node(s) for initial structure
INIT_LAYER_Z = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
N_WINDOW = 300 # 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.00004 # Network optimization learning rate
#LEARNING_RATE = 0.01 # Network optimization learning rate
# LEARNING_RATE_X = 0.02 # Network optimization learning rate 0.00085
# LEARNING_RATE_Y = 0.02 # Network optimization learning rate 0.00085
# LEARNING_RATE_Z = 0.05 # Network optimization learning rate
LEARNING_RATE_X = 0.0125 # Network optimization learning rate 0.00085
LEARNING_RATE_Y = 0.0125 # Network optimization learning rate 0.00085
LEARNING_RATE_Z = 0.06 # Network optimization learning rate
WEIGHT_DECAY = 0.000125 # Regularization weight decay factor
#WEIGHT_DECAY = 0.0 # Regularization weight decay factor
#SLIDING_WINDOW = 35
# INITIALIZE SYSTEM AND SIMULATION PARAMETERS:
IRIS_THRUST = 0.5629 # Nominal thrust for IRIS Quadrotor
RATE = 25.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.45,0.0,0.002] # PID Gain Parameter [KP,KI,KD] axis-X
# PID_GAIN_Y = [0.45,0.0,0.002] # PID Gain Parameter [KP,KI,KD] axis-Y
PID_GAIN_X = [0.45, 0.0, 0.0] # PID Gain Parameter [KP,KI,KD] axis-X
PID_GAIN_Y = [0.45, 0.0, 0.0] # 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.85,0.0,0.0001] # PID Gain Parameter [KP,KI,KD] axis-Z
PID_GAIN_Z = [0.85, 0.0, 0.0] # PID Gain Parameter [KP,KI,KD] axis-Z
SIM_TIME = 295 # Simulation time duration (s)
#--------------------------------------------------------------------------------
# Initial conditions of UAV system
xref = 0.0
yref = 0.0
zref = 1.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_X)
Ycon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE_Y)
Zcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE_Z)
if INIT_LAYER_X > 2:
for i in range(INIT_LAYER_X - 2):
Xcon.addhiddenlayer()
if INIT_LAYER_Y > 2:
for i in range(INIT_LAYER_Y - 2):
Ycon.addhiddenlayer()
if INIT_LAYER_Z > 2:
for i in range(INIT_LAYER_Z - 2):
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
#Init Weight
# Wx0=torch.tensor([[ 0.3051, 0.3059, 0.3048, -0.0287],
# [ 0.3099, 0.3084, 0.3077, -0.0291],
# [ 0.3097, 0.3088, 0.3063, -0.0300]], dtype=torch.float64,requires_grad = True)
# Wx1=torch.tensor([[0.4430, 0.4474, 0.4469],
# [0.3885, 0.3906, 0.3904],
# [0.3092, 0.3079, 0.3083]], dtype=torch.float64,requires_grad = True)
# Wx2=torch.tensor([[0.7390, 0.5607, 0.3045]], dtype=torch.float64,requires_grad = True)
# Wy0=torch.tensor([[-0.0770, -0.0733, -0.0730, 0.0026],
# [-0.3706, -0.3641, -0.3646, 0.0189]], dtype=torch.float64,requires_grad = True)
# Wy1=torch.tensor([[-0.2303, -1.1563]], dtype=torch.float64,requires_grad = True)
# # Wz0=torch.tensor([[-0.3250, -0.3174, -0.3083, 0.0508]], dtype=torch.float64,requires_grad = True)
# # Wz1=torch.tensor([[-0.5591]], dtype=torch.float64,requires_grad = True)
# Xcon.netStruct[0].linear.weight.data = Wx0
# Xcon.netStruct[1].linear.weight.data = Wx1
# Xcon.netStruct[2].linear.weight.data = Wx2
# Ycon.netStruct[0].linear.weight.data = Wy0
# Ycon.netStruct[1].linear.weight.data = Wy1
# Zcon.netStruct[0].linear.weight.data = Wz0
# Zcon.netStruct[1].linear.weight.data = Wz1
# PX4 API object
modes = fcuModes() # Flight mode object
uav = uavCommand() # UAV command object
trajectory = Trajectory()
meanErrX = recursiveMean()
meanErrY = recursiveMean()
meanErrZ = recursiveMean()
# Data Logger object
logData = dataLogger('flight')
xconLog = dataLogger('X')
yconLog = dataLogger('Y')
zconLog = dataLogger('Z')
# 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)
# Setpoint publisher
velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
# Velocity messages init
vel = TwistStamped()
vel.twist.linear.x = 0.0
vel.twist.linear.y = 0.0
vel.twist.linear.z = 0.0
# Arming the UAV --> no auto arming, please comment out line 140
text = colored("Ready for Flight..Press Enter to continue...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
k = 0
while not uav.state.armed:
#modes.setArm()
rate.sleep()
k += 1
if k % 10 == 0:
text = colored('Waiting for arming..', 'yellow')
print(text)
if k > 500:
text = colored('Arming timeout..',
'red',
attrs=['reverse', 'blink'])
print(text)
break
# 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 not arming..', 'yellow')
# print (text)
rate.sleep()
k = 0
while k < 10:
velocity_pub.publish(vel)
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:
# Take off with altitude Z = 1 m
if runtime <= 15:
xref = 0.0
yref = 0.0
zref = 0.3 + trajectory.linear(0.7, dt, 15.0)
if runtime == 15:
trajectory.reset()
j = 0
xs = uav.local_pos.x
xr = np.linspace(xs, -0.9, num=(5.0 / dt))
# Go to X=-0.9 direction
if runtime > 15 and runtime <= 20:
xref = xr[j]
yref = 0.0
zref = 1.0
j += 1
if runtime == 20:
trajectory.reset()
ys = uav.local_pos.y
j = 0
yr = np.linspace(ys, 1.0, num=(5.0 / dt))
# Go to Y =1 direction
if runtime > 20 and runtime <= 25:
xref = -0.9
yref = yr[j]
zref = 1.0
j += 1
if runtime == 25:
trajectory.reset()
zs = uav.local_pos.z
zr = np.linspace(zs, 1.3, num=(5.0 / dt))
j = 0
# Go to Z= 1.3
if runtime > 25 and runtime <= 30:
xref = -0.9
yref = 1.0
zref = zr[j]
j += 1
if runtime == 30:
trajectory.reset()
j = 0
# Hold 5 s
if runtime > 30 and runtime <= 35:
xref = -0.9
yref = 1.0
zref = 1.3
if runtime == 35:
trajectory.reset()
j = 0
# Sinusoidal Z trajectory after 10 times
if runtime > 35 and runtime <= 185:
xref = -0.9
yref = 1.0
zref = 1.3 + trajectory.sinusoid(0.5, dt, 15)
if runtime == 185:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 1.0, num=(5.0 / dt))
# Go to Z=1.0
if runtime > 185 and runtime <= 190:
xref = -0.9
yref = 1.0
zref = zr[j]
j += 1
if runtime == 190:
trajectory.reset()
xs = uav.local_pos.x
ys = uav.local_pos.y
j = 0
yr = np.linspace(ys, -1.67, num=(10.0 / dt))
# Go to Y=-1.65
if runtime > 190 and runtime <= 200:
xref = -0.9
yref = yr[j]
zref = 1.0
j += 1
if runtime == 200:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 1.3, num=(5.0 / dt))
# Go to Z= 1.3
if runtime > 200 and runtime <= 205:
xref = -0.9
yref = -1.67
zref = zr[j]
j += 1
if runtime == 205:
trajectory.reset()
j = 0
# Hold 5 s
if runtime > 205 and runtime <= 210:
xref = -0.9
yref = -1.67
zref = 1.3
if runtime == 205:
trajectory.reset()
j = 0
# Approach Ceiling +- 0.07 4 times
if runtime > 210 and runtime <= 275:
xref = -0.9
yref = -1.67
zref = 1.3 + trajectory.sinusoid(0.07, dt, 15)
if runtime == 275:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 1.0, num=(5.0 / dt))
# Go to Z = 1.0
if runtime > 275 and runtime <= 280:
xref = -0.9
yref = -1.67
zref = zr[j]
j += 1
if runtime == 280:
trajectory.reset()
ys = uav.local_pos.y
j = 0
yr = np.linspace(ys, 1.0, num=(5.0 / dt))
# Go to Y= 1
if runtime > 280 and runtime <= 285:
xref = -0.9
yref = yr[j]
zref = 1.0
j += 1
if runtime == 285:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 0.4, num=(10.0 / dt))
#Landing
if runtime > 285:
xref = -0.9
yref = 1.0
zref = zr[j]
j += 1
# 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
vz = Zcon.controlUpdate(zref) # Additional Thrust Z
vx = limiter(vx, 1)
vy = limiter(vy, 1)
vz = limiter(vz, 1)
# Publish the control signal
vel.twist.linear.x = vx
vel.twist.linear.y = vy
vel.twist.linear.z = vz
velocity_pub.publish(vel)
# NN Learning stage
# if meanErrX.updateMean(abs(errx),0.99) > 0.05:
# Xcon.optimize(LEARNING_RATE_X,WEIGHT_DECAY)
# if meanErrY.updateMean(abs(erry),0.99) > 0.05:
# Ycon.optimize(LEARNING_RATE_Y,WEIGHT_DECAY)
# if meanErrZ.updateMean(abs(errz),0.99) > 0.05:
# Zcon.optimize(LEARNING_RATE_Z,WEIGHT_DECAY)
Xcon.optimize(LEARNING_RATE_X, WEIGHT_DECAY)
Ycon.optimize(LEARNING_RATE_Y, WEIGHT_DECAY)
Zcon.optimize(LEARNING_RATE_Z, WEIGHT_DECAY)
# 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)
#euler = euler_from_quaternion(uav.q)
# 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('ex, ey, ez = ', errx, erry, errz)
print('vx,vy,vz = ', vx, vy, vz)
#print 'att angles = ',euler
print('Nodes X Y Z = ', Xcon.nNodes, Ycon.nNodes, Zcon.nNodes)
print('Layer X Y Z = ', Xcon.nLayer, Ycon.nLayer, Zcon.nLayer)
# print Ycon.netStruct[0].linear.weight.data
# print Ycon.netStruct[1].linear.weight.data
print('')
k += 1
runtime = k * dt
execTime = time.time() - start
print('Runtime = ', runtime)
print('Exec time = ', execTime)
print('')
# Append logged Data
logData.appendStateData(runtime, execTime, xref, yref, zref, xpos,
ypos, zpos, vx, vy, vz)
xconLog.appendControlData(runtime, Xcon.us, Xcon.un, Xcon.nLayer,
Xcon.nNodes)
yconLog.appendControlData(runtime, Ycon.us, Ycon.un, Ycon.nLayer,
Ycon.nNodes)
zconLog.appendControlData(runtime, Zcon.us, Zcon.un, Zcon.nLayer,
Zcon.nNodes)
# Save logged Data
logData.saveStateData(runtime, execTime, xref, yref, zref, xpos, ypos,
zpos, vx, vy, vz)
xconLog.saveControlData(runtime, Xcon.us, Xcon.un, Xcon.nLayer,
Xcon.nNodes)
yconLog.saveControlData(runtime, Ycon.us, Ycon.un, Ycon.nLayer,
Ycon.nNodes)
zconLog.saveControlData(runtime, Zcon.us, Zcon.un, Zcon.nLayer,
Zcon.nNodes)
# Break condition
if runtime > SIM_TIME:
# Set auto land mode
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)
# Closing the saved log files
logData.logFile.close()
xconLog.logFile.close()
yconLog.logFile.close()
zconLog.logFile.close()
# Save network parameters
saveParameters("ceiling_evo", Xcon.netStruct, Ycon.netStruct,
Zcon.netStruct)
# Plotting the results
# logData.plotFigure()
# xconLog.plotControlData()
# yconLog.plotControlData()
# zconLog.plotControlData()
text = colored('Mission Complete!!',
'green',
attrs=['reverse', 'blink'])
print(text)
break
def create_twist(self, velocity, angular):
tw = TwistStamped()
tw.twist.linear.x = velocity
tw.twist.angular.z = angular
return tw
def start_path(way_points):
#draw the path
draw_path(way_points)
#Get the number to break up the path
div = len(way_points.path) / way_points.sample
destination = []
vel = Twist()
my_vel = []
#get all the way points
for i in xrange(div - 1):
destination.append(way_points.path[i * way_points.sample])
destination.append(way_points.path[-1])
#move through all the points
for i in xrange(len(destination) - 1):
#write data to a CSV file
#write(destination[i])
# Get the coefs of the polynomial
###to close the loop change destination[i] to output of EKF
# to use a 3rd order fit
#(a0,a1,a2,a3 ) = make_poly_3rd_order( destination[i], destination[i+1])
# to use a 5th order fit
(a0, a1, a2, a3, a4, a5) = make_poly_5th_order(destination[i],
destination[i + 1])
#start adn end tiem
t1 = float(destination[i].time)
t2 = float(destination[i + 1].time)
print "t2 - t1 " + str(float(t2) - t1)
t = 0
# time step
#send time to EQ
while (t < float(t2) - t1):
t = t + EKF.my_dt
print t
my_vel = []
pose_output = []
#print t
#move through x,y,theta
for d in xrange(len(a1)):
#use the deritive of the EQ to get the velocity in each direction
#3rd order
#temp = a1[d] + 2*a2[d]*(t) + 3*a3[d]*((t)**2)
#temp_pose = a0[d] + a1[d]*t + a2[d]*(t)**2 + a3[d]*((t)**3)
#5th order
temp = a1[d] + 2 * a2[d] * (t) + 3 * a3[d] * (
(t)**2) + 4 * a4[d] * ((t)**3) + 5 * a5[d] * ((t)**4)
temp_pose = a0[d] + a1[d] * t + a2[d] * (t)**2 + a3[d] * (
(t)**3) + a4[d] * ((t)**4) + a5[d] * ((t)**5)
my_vel.append(temp)
pose_output.append(temp_pose)
pose_output.append(destination[i].pose.position.x)
pose_output.append(destination[i].pose.position.y)
outputWriter.writerow(pose_output)
#publish the velocity
vel = Twist()
vel_stamp = TwistStamped()
vel.linear.x = my_vel[0]
vel.linear.y = my_vel[1]
vel.angular.z = my_vel[2]
#print my_vel[1]
vel_stamp.header.stamp = rospy.Time.now()
vel_stamp.twist = vel
path_vel.publish(vel)
stamp_vel.publish(vel_stamp)
#simulate the time
rospy.sleep(EKF.my_dt)
#close the file
print "done"
outputFile.close()
