def __init__(self, config):
self.alt_topic = config["alt_topic"]
self.gps_topic = config["gps_topic"]
self.state_topic = config["state_topic"]
self.armed = False
self.connected = False
self.mode = ''
self.takeoff = False
self.latitude = 0
self.longitude = 0
self.altitude = 0
rospy.init_node("mavros_vehicle", anonymous=True)
mavros.set_namespace()
if mavros.utils.wait_fcu_connection(timeout=10):
print("Connected to Flight Controller !")
self.connected = True
CreateThread(self.heartbeat)
print("Listening to Altitude on %s" % self.alt_topic)
print("Listening to GPS on - %s" % self.gps_topic)
print("Listening to State on - %s" % self.state_topic)
param.param_set("NAV_RCL_ACT", 0)
param.param_set("NAV_DLL_ACT", 0)
def __init__(self, hz=50):
rospy.loginfo("ObstaclePathPlanner Started!")
mavros.set_namespace()
self.ar_pose_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers,
self.ar_pose_cb)
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped, self.local_pose_cb)
self.z_error_pub = rospy.Publisher("/error/z", Float64, queue_size=1)
""" State:
0: Hovering in place
1: Line following
2: Going over obstacles
3: Going under obstacles
"""
self.state = 1
self.rate = rospy.Rate(hz)
self.hz = hz
# Async variables:
self.markers = []
self.current_pose = None
# Vars for states 2 & 3:
self.current_obstacle = None
self.current_obstacle_dist = None
self.artag_seen_x = None
self.send_state()
def main():
listener()
rospy.init_node('FLYtoPOS')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(25)
rospy.loginfo("Starting...")
position_msg = PoseStamped()
position_msg.header.frame_id = "home"
x = 1
y = 1
z = 25
while not rospy.is_shutdown():
position_msg.header.stamp = rospy.Time.now()
position_msg.pose.position.x = x
position_msg.pose.position.y = y
position_msg.pose.position.z = z
pubPosition.publish(position_msg)
rate.sleep()
rospy.loginfo("Bye!")
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(10)
setpoint = SetpointPosition()
rospy.loginfo("Climb")
setpoint.set(0.0, 0.0, 3.0, 0)
setpoint.set(0.0, 0.0, 10.0, 5)
rospy.loginfo("Sink")
setpoint.set(0.0, 0.0, 8.0, 5)
rospy.loginfo("Fly to the right")
setpoint.set(10.0, 4.0, 8.0, 5)
rospy.loginfo("Fly to the left")
setpoint.set(0.0, 0.0, 8.0, 5)
offset_x = 0.0
offset_y = 0.0
offset_z = 10.0
sides = 360
radius = 20
rospy.loginfo("Fly in a circle")
setpoint.set(0.0, 0.0, 10.0, 3) # Climb to the starting height first
i = 0
while not rospy.is_shutdown():
x = radius * cos(i * 2 * pi / sides) + offset_x
y = radius * sin(i * 2 * pi / sides) + offset_y
z = offset_z
wait = False
delay = 0
if (i == 0 or i == sides):
# Let it reach the setpoint.
wait = True
delay = 5
setpoint.set(x, y, z, delay, wait)
i = i + 1
rate.sleep()
if (i > sides):
rospy.loginfo("Fly home")
setpoint.set(0.0, 0.0, 10.0, 5)
break
# Simulate a slow landing.
setpoint.set(0.0, 0.0, 8.0, 5)
setpoint.set(0.0, 0.0, 3.0, 5)
setpoint.set(0.0, 0.0, 2.0, 2)
setpoint.set(0.0, 0.0, 1.0, 2)
setpoint.set(0.0, 0.0, 0.0, 2)
setpoint.set(0.0, 0.0, -0.2, 2)
rospy.loginfo("Bye!")
def start():
rospy.init_node(name())
mavros.set_namespace()
global set_mode, set_param, arming, set_home, takeoff, command, land, wp_push, wp_clear, wp_set
set_mode = get_proxy('/mavros/set_mode', SetMode)
set_param = get_proxy('/mavros/param/set', ParamSet)
arming = get_proxy('/mavros/cmd/arming', CommandBool)
set_home = get_proxy('/mavros/cmd/set_home', CommandHome)
command = get_proxy('/mavros/cmd/command', CommandLong)
takeoff = get_proxy('/mavros/cmd/takeoff', CommandTOL)
land = get_proxy('/mavros/cmd/land', CommandTOL)
wp_push = get_proxy('/mavros/mission/push', WaypointPush)
wp_clear = get_proxy('/mavros/mission/clear', WaypointClear)
wp_set = get_proxy('/mavros/mission/set_current', WaypointSetCurrent)
global gps_topic, imu_sub
gps_topic = mavros.get_topic('global_position', 'global')
rospy.Service('command/takeoff', TakeOff, handle_takeoff)
rospy.Service('command/land', Land, handle_land)
rospy.Service('command/flyto', FlyTo, handle_flyto)
rospy.Service('command/fly_waypoints', FlyWaypoints, handle_flywaypoints)
rospy.Subscriber("/mavros/global_position/global", NavSatFix,
partial(values.latch_value, gps_topic, max_age=10))
# Wait to receive some data from mavros before intializing
imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, handle_startup)
rospy.spin()
def main():
rospy.init_node('follow_ar')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(25)
rospy.loginfo("Starting...")
position_msg = PoseStamped()
position_msg.header.frame_id = "copter_home"
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/copter_home', '/ar_marker_13', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
position_msg.header.stamp = rospy.Time.now()
position_msg.pose.position.x = trans[0]
position_msg.pose.position.y = trans[1]
position_msg.pose.position.z = trans[2] + 5
pubPosition.publish(position_msg)
rate.sleep()
rospy.loginfo("Bye!")
def __init__(self, rate=10):
""" Initializes publishers and subscribers, sets initial values for vars
:param rate: the rate at which the setpoint_velocity is published
"""
assert rate > 2 # make sure rate is high enough, if new setpoint recieved within .5 seconds, robot will switch back to POSCTL
self.rate = rospy.Rate(rate)
mavros.set_namespace()
self.bridge = CvBridge()
self.pub_local_velocity_setpoint = rospy.Publisher(
"/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=1)
self.sub_line_param = rospy.Subscriber("/line/param", Line,
self.line_param_cb)
self.pub_error = rospy.Publisher("/line/error", Vector3, queue_size=1)
# Variables dealing with publishing setpoint
self.sub_state = rospy.Subscriber("/mavros/state", State,
self.state_cb)
self.current_state = None
self.offboard_point_streaming = False
# Setpoint field expressed as the desired velocity of the body-down frame
# with respect to the world frame parameterized in the body-down frame
self.velocity_setpoint = None
def __init__(self):
rospy.init_node('listener_results', anonymous=True)
self.rate = rospy.Rate(20.0) # MUST be more then 2Hz
mavros.set_namespace('mavros')
state_sub = rospy.Subscriber(mavros.get_topic('state'),
mavros_msgs.msg.State, self.state_cb)
# "/mavros/setpoint_velocity/cmd_vel"
vel_pub_sub = rospy.Subscriber(
mavros.get_topic('setpoint_velocity', 'cmd_vel'), TwistStamped,
self.vel_pub_cb)
vel_local_sub = rospy.Subscriber(
mavros.get_topic('local_position', 'velocity_local'), TwistStamped,
self.vel_local_cb)
self.current_state = mavros_msgs.msg.State()
self.vel_pub = [0.0] * 4
self.vel_local = [0.0] * 4
self.vel_pub_x = []
self.vel_pub_y = []
self.vel_pub_z = []
self.vel_local_x = []
self.vel_local_y = []
self.vel_local_z = []
#rospy.spin()
self.show()
self.plot()
def main():
rospy.init_node('FLYtoPOS')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(25)
rospy.loginfo("Starting...")
position_msg = PoseStamped()
position_msg.header.frame_id = "copter_home"
x = [0.0, 0.0, 5.0, 5.0, 0.0]
y = [0.0, 5.0, 5.0, 0.0, 0.0]
z = [5.0, 5.0, 5.0, 5.0, 0.0]
while not rospy.is_shutdown():
global iterator
position_msg.header.stamp = rospy.Time.now()
position_msg.pose.position.x = x[iterator]
position_msg.pose.position.y = y[iterator]
position_msg.pose.position.z = z[iterator]
pubPosition.publish(position_msg)
rate.sleep()
rospy.loginfo("Bye!")
def main():
rospy.init_node('FLYtoPOS')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(25)
rospy.loginfo("Starting...")
print("Starting...")
position_msg = PoseStamped()
position_msg.header.frame_id = "home"
global target_position_x
global target_position_y
while not rospy.is_shutdown():
position_msg.header.stamp = rospy.Time.now()
position_msg.pose.position.x = target_position_x
position_msg.pose.position.y = target_position_y
position_msg.pose.position.z = 8
pubPosition.publish(position_msg)
print(position_msg)
rate.sleep()
rospy.loginfo("Bye!")
def __init__(self, filename=None):
self.ser = serial.Serial(SERIAL_PORT, 9600)
if filename:
self.log_file = open("logs/%s_log.csv" % filename, 'w')
else:
self.log_file = open(
"logs/%s_log.csv" %
datetime.datetime.now().strftime('%m%d%H%M%S'), 'w')
self.log_file.write(
"timestamp,vel_x,vel_y,vel_z,acc_x,acc_y,acc_z,roll,pitch,yaw,rc0,rc1,rc2,rc3,vol,cur_raw,cur,power,act_vx,act_vy,act_vz,mode\n"
)
mavros.set_namespace()
rospy.init_node('aero_energy_logger')
self.cur_state = State()
self.velocity = TwistStamped()
self.rc_in = RCIn()
self.imu = Imu()
self.manual_control = ManualControl()
self.battery_state = BatteryState()
self.pose = PoseStamped()
self.cur_val_list = [0] * FIELD_NUM
print "initialize subscribers..."
self.init_subscribers()
print "initialize services..."
self.init_services()
print "initialize publishers..."
self.init_publishers()
self.start_time = time.time()
def __init__(self):
rospy.init_node('gps_goto')
mavros.set_namespace() # initialize mavros module with default namespace
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.current_lat = 0.0
self.current_lon = 0.0
self.current_alt = 0.0
self.throttle_update = 0.0
self.attitude_publish = True
self.pid_throttle = pid_controller.PID(P = 0.8, I = 0.8, D = 0.8, Integrator_max=1, Integrator_min=0)
self.pid_throttle.setPoint(10)
self.setHome = False
self.home_lat = 0.0
self.home_lon = 0.0
self.home_alt = 0.0
self.publish = True
self.velocity_init()
def __init__(self, copter_id = "1", mavros_string="/mavros/copter1"):
rospy.init_node('velocity_goto_'+copter_id)
mavros.set_namespace(mavros_string) # initialize mavros module with default namespace
self.pid_alt = pid_controller.PID()
self.mavros_string = mavros_string
self.final_alt = 0.0
self.final_pos_x = 0.0
self.final_pos_y = 0.0
self.final_vel = 0.0
self.cur_rad = 0.0
self.cur_alt = 0.0
self.cur_pos_x = 0.0
self.cur_pos_y = 0.0
self.cur_vel = 0.0
self.vx = 0.0
self.vy = 0.0
self.vz = 0.0
self.alt_control = False
self.override_nav = False
self.reached = False
self.done = False
self.last_sign_dist = 0.0
def __init__(self):
rospy.init_node('gps_goto')
mavros.set_namespace(
) # initialize mavros module with default namespace
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.current_lat = 0.0
self.current_lon = 0.0
self.current_alt = 0.0
self.throttle_update = 0.0
self.attitude_publish = True
self.pid_throttle = pid_controller.PID(P=0.8,
I=0.8,
D=0.8,
Integrator_max=1,
Integrator_min=0)
self.pid_throttle.setPoint(10)
self.setHome = False
self.home_lat = 0.0
self.home_lon = 0.0
self.home_alt = 0.0
self.publish = True
self.velocity_init()
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
mavros.set_namespace("/mavros") # initialize mavros module with default namespace
rate = rospy.Rate(10)
setpoint = SetpointPosition()
rospy.loginfo("Climb")
setpoint.set(0.0, 0.0, 3.0, 0)
setpoint.set(0.0, 0.0, 10.0, 5)
rospy.loginfo("Sink")
setpoint.set(0.0, 0.0, 8.0, 5)
rospy.loginfo("Fly to the right")
setpoint.set(10.0, 4.0, 8.0, 5)
rospy.loginfo("Fly to the left")
setpoint.set(0.0, 0.0, 8.0, 5)
offset_x = 0.0
offset_y = 0.0
offset_z = 10.0
sides = 360
radius = 20
rospy.loginfo("Fly in a circle")
setpoint.set(0.0, 0.0, 10.0, 3) # Climb to the starting height first
i = 0
while not rospy.is_shutdown():
x = radius * cos(i * 2 * pi / sides) + offset_x
y = radius * sin(i * 2 * pi / sides) + offset_y
z = offset_z
wait = False
delay = 0
if (i == 0 or i == sides):
# Let it reach the setpoint.
wait = True
delay = 5
setpoint.set(x, y, z, delay, wait)
i = i + 1
rate.sleep()
if (i > sides):
rospy.loginfo("Fly home")
setpoint.set(0.0, 0.0, 10.0, 5)
break
# Simulate a slow landing.
setpoint.set(0.0, 0.0, 8.0, 5)
setpoint.set(0.0, 0.0, 3.0, 5)
setpoint.set(0.0, 0.0, 2.0, 2)
setpoint.set(0.0, 0.0, 1.0, 2)
setpoint.set(0.0, 0.0, 0.0, 2)
setpoint.set(0.0, 0.0, -0.2, 2)
rospy.loginfo("Bye!")
def __init__(self):
if not MAVLINK:
mavros.set_namespace()
self.last_rcio_power = RCIOPower()
self.publisher = rospy.Publisher('/mavros/rc/override',
mavros_msgs.msg.OverrideRCIn)
self.in_subscriber = rospy.Subscriber('/mavros/rc/in',
mavros_msgs.msg.RCIn,
callback=log)
self.out_subscriber = rospy.Subscriber('/mavros/rc/out',
mavros_msgs.msg.RCOut,
callback=log)
self.arm_function = rospy.ServiceProxy(
mavros.get_topic('cmd', 'arming'),
mavros_msgs.srv.CommandBool).call
alt_hold_msg = mavros_msgs.msg.OverrideRCIn()
alt_hold_msg.channels = [
0xffff, 0xffff, 0xffff, 0xffff, 1500, 0xffff, 0xffff, 0xffff
]
self.publisher.publish(alt_hold_msg)
else:
self.master = mavutil.mavlink_connection("/dev/ttyACM0",
baud=57600)
print("waiting for heartbeat...")
self.master.wait_heartbeat()
print("Connected")
self.master.mav.request_data_stream_send(
master.target_system, self.master.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL, 4, 1)
def __init__(self):
# Arm the drone
mavros.set_namespace()
command.arming(True)
self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.wait_for_service('mavros/set_mode')
change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.init_node('gotowaypoint', anonymous=True)
rate = rospy.Rate(50)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_cb)
self.waypoint = PoseStamped()
self.waypoint.header.frame_id = 'map'
self.waypoint.pose.position.x = 0
self.waypoint.pose.position.y = 0
self.waypoint.pose.position.z = 5
self.marker_pub = rospy.Publisher('/waypoint_marker', Marker, queue_size=10)
self.waypoint_marker = Marker()
self.waypoints = np.array([[0,0,0],
[0,1,1],
[0,3,3]])
self.publish_waypoint()
while not rospy.is_shutdown():
result_mode = change_mode(0,"OFFBOARD")
self.pub_sp.publish(self.waypoint)
rate.sleep()
def __init__(self, collect=False):
rospy.loginfo("Starting robot...")
self.controls = [0, 0, 0, 0, 0, 0, 0]
self.fwd_image_sub = rospy.Subscriber("camera/image_raw", Image,
self.fwd_image_cb)
self.dwn_image_sub = rospy.Subscriber("webcam/image_raw", Image,
self.dwn_image_cb)
self.controls_sub = rospy.Subscriber("mavros/rc/out", RCOut,
self.controls_cb)
self.arm_srv = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
self.motor_pub = rospy.Publisher("/mavros/rc/override",
OverrideRCIn,
queue_size=1000)
self.bridge = CvBridge()
self.fwd_img_num = 0
self.dwn_img_num = 0
self.fwd_img_csv_fname = "/media/nvidia/fwd_img_timestamp.csv"
self.dwn_img_csv_fname = "/media/nvidia/dwn_img_timestamp.csv"
self.img_path = "/media/nvidia/images/"
self.data_collection = collect
if self.data_collection:
f = open(self.fwd_img_csv_fname, "w+")
f.write(
"timestamp (seconds from epoch), forward camera image filename\n"
)
f.close()
f = open(self.dwn_img_csv_fname, "w+")
f.write(
"timestamp (seconds from epoch), downward camera image filename\n"
)
f.close()
mavros.set_namespace()
self.armed = False
rospy.wait_for_service("/mavros/cmd/arming")
def __init__(self):
# Reqired call - I think if you are running more than one drone it allows you to give them different topic namespaces, but defaults to mavros
mavros.set_namespace()
self._curr_state = State()
self._is_done = False
self.flight_path = []
self.rate = rospy.Rate(20)
self.nextPos = PoseStamped()
self.lat = 0
self.lng = 0
# Various ros publishers and subscribers - somewhat self documenting
self.gps_subscriber = rospy.Subscriber(
mavros.get_topic('global_position', 'global'), NavSatFix,
self._get_fix)
self.state_subscriber = rospy.Subscriber("mavros/state", State,
self._update_state)
self._local_position_publisher = rospy.Publisher(
"mavros/setpoint_position/local", PoseStamped, queue_size=10)
self._arming_client = rospy.ServiceProxy("mavros/cmd/arming",
CommandBool)
self._set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
self._takeoff_client = rospy.ServiceProxy("mavros/cmd/takeoff",
CommandTOL)
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/ardrone/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
time.sleep(1)
print setpoint.x, setpoint.y
rospy.loginfo("Climb")
setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 5.0, 2)
setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 8.0, 5)
setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 10.0, 5)
setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 15.0, 5)
setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 18.0, 5)
setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 20.0, 5)
rospy.loginfo("Sink")
setpoint.set(0.0, 0.0, 20.0, 5)
# Simulate a slow landing.
setpoint.set(0.0, 0.0, 18.0, 5)
setpoint.set(0.0, 0.0, 15.0, 5)
setpoint.set(0.0, 0.0, 10.0, 2)
setpoint.set(0.0, 0.0, 8.0, 5)
setpoint.set(0.0, 0.0, 5.0, 5)
setpoint.set(0.0, 0.0, -0.2, 5)
rospy.loginfo("Bye!")
def main():
rospy.init_node('default_objrecog', anonymous=True)
rate = rospy.Rate(10)
mavros.set_namespace('/mavros')
subscriber = rospy.Subscriber("/iris/camera/image_raw",
Image,
callback,
queue_size=1)
pub_cent = rospy.Publisher(
'/centroid/location', String,
queue_size=10) #/object/centroid #string and not Float
pub_stat = rospy.Publisher('marker/location',
Float32MultiArray,
queue_size=10)
#print("test")
while not rospy.is_shutdown():
#rospy.loginfo(centroids_str)
#pub_cent.publish(centroids_str)
pub_stat.publish(centroids_float)
rate.sleep()
# k = cv2.waitKey(1) & 0xff
# if k == 27: break
return 0
def __init__(self, namespace="mavros"):
self.rate = rospy.Rate(20)
self.current_pose = PoseStamped()
self.current_velocity = TwistStamped()
self.target_pose = PoseStamped()
self.UAV_state = mavros_msgs.msg.State()
mavros.set_namespace(namespace)
# setup subscriber
self._state_sub = rospy.Subscriber(mavros.get_topic('state'), State,
self._state_callback)
self._local_position_sub = rospy.Subscriber(
mavros.get_topic('local_position', 'pose'), PoseStamped,
self._local_position_callback)
self._local_velocity_sub = rospy.Subscriber(
mavros.get_topic('local_position', 'velocity_body'), TwistStamped,
self._local_velocity_callback)
# setup publisher
self._setpoint_local_pub = mavros.setpoint.get_pub_position_local(
queue_size=10)
# setup service
self.set_arming = rospy.ServiceProxy(mavros.get_topic('cmd', 'arming'),
mavros_msgs.srv.CommandBool)
self.set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'),
mavros_msgs.srv.SetMode)
def main():
# Initialize Node class
parser = argparse.ArgumentParser(
description="Teleoperation script for Copter-UAV")
rospy.init_node('px4_control', anonymous=True)
parser.add_argument('-n',
'--mavros-ns',
help="ROS node namespace",
default="/mavros")
global args
args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
threading.Thread(target=velocity_call).start()
mavros.set_namespace(args.mavros_ns)
#orig_settings = termios.tcgetattr(sys.stdin)
#tty.setraw(sys.stdin)
px4C = px4_planner()
# spin the node
try:
rospy.spin()
# # stop the node if Ctrl-C pressed
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
def setpoint_demo():
rospy.init_node('setpoint_position_demo_1')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/uav_1/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
time.sleep(1)
rospy.loginfo("Climb")
setpoint.takeoff(1.0)
# setpoint.navigate()
rospy.loginfo("Moving to Pose 1")
#setpoint.setPose(0,0,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(7,-1,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(7.862669,-8.131087,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3.296570,-8.891940,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3.413211,-6,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-4.830871,-6,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-6.704798,-6,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-6.692234,-8,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-6.362257,7.847113,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-4.927931,5.589492,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-2.475843,6.678944,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3,8.609401,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(8.157134,8.609435,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(8.201663,1.988020,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3,1.942599,1,5)
rospy.loginfo("Landing")
setpoint.land()
rospy.loginfo("Bye!")
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
mavros.set_namespace()
rate = rospy.Rate(10)
setpoint = SetPointPosition()
rospy.loginfo("Climb")
setpoint.set(0.0, 0.0, 0.5, 0)
setpoint.set(0.0, 0.0, 2.0, 5)
rospy.loginfo("Sink")
setpoint.set(0.0, 0.0, 1.0, 5)
rospy.loginfo("Climb")
setpoint.set(0.0, 0.0, 2.0, 5)
rospy.loginfo("Fly to the right")
setpoint.set( 10.0, 4.0, 8.0, 5)
rospy.loginfo("Fly to the left")
setpoint.set( 0.0, 0.0, 8.0, 5)
rospy.loginfo("Landing")
setpoint.set(0.0, 0.0, 8.0, 5)
setpoint.set(0.0, 0.0, 3.0, 5)
setpoint.set(0.0, 0.0, 2.0, 2)
setpoint.set(0.0, 0.0, 1.0, 2)
setpoint.set(0.0, 0.0, 0.0, 2)
setpoint.set(0.0, 0.0, -0.2, 2)
def main():
rospy.init_node('FLYtoPOS')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(25)
rospy.loginfo("Starting...")
print("Starting...")
position_msg = PoseStamped()
position_msg.header.frame_id = "home"
global target_position_x
global target_position_y
global target_position_z
global time_to_next_z_change
global transformData
transformData = TransformStamped() #setup object to receive transform
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer) #setup tf listener
while not rospy.is_shutdown():
#lookup for the newest transform
#transformData = tfBuffer.lookup_transform('usb_camera::camera_link', 'ar_marker_13', rospy.Time(0))
try:
transformData = tfBuffer.lookup_transform('map', 'ar_marker_13',
rospy.Time(0))
follow_ar()
print transformData
print '########'
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
pass
time_now = time.time()
if (time_now - time_prev) >= time_to_next_z_change:
time_to_next_z_change = 1
if position_msg.pose.position.z < 5:
target_position_z = target_position_z - 1
else:
target_position_z = target_position_z - 0.2
if position_msg.pose.position.z < 0.1:
target_position_z = 0
position_msg.header.stamp = rospy.Time.now()
position_msg.pose.position.x = target_position_x
position_msg.pose.position.y = target_position_y
position_msg.pose.position.z = target_position_z
print target_position_x
print target_position_y
print target_position_z
pubPosition.publish(position_msg)
#print(position_msg)
rate.sleep()
rospy.loginfo("Bye!")
def __init__(self, actionServer, goal):
'''Initialize ros publisher, ros subscriber'''
rospack = rospkg.RosPack()
packagePath = rospack.get_path('kuri_object_tracking')
with open(packagePath+'/config/colors.yaml', 'r') as stream:
try:
config_yaml = yaml.load(stream)
self.H_red = config_yaml.get('H_red')
self.S_red = config_yaml.get('S_red')
self.V_red = config_yaml.get('V_red')
self.H_blue = config_yaml.get('H_blue')
self.S_blue = config_yaml.get('S_blue')
self.V_blue = config_yaml.get('V_blue')
self.H_green = config_yaml.get('H_green')
self.S_green = config_yaml.get('S_green')
self.V_green = config_yaml.get('V_green')
except yaml.YAMLError as exc:
print(exc)
self.navigate_started = False
self.bridge = CvBridge()
self.actionServer = actionServer
self.obstacles = Objects()
self.image_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/image', Image)
self.info_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/camera_info', CameraInfo)
#self.image_sub = message_filters.Subscriber('/usb_cam/image_raw', Image)
#self.info_sub = message_filters.Subscriber('/usb_cam/camera_info', CameraInfo)
#self.outpub = rospy.Publisher('/uav_'+str(goal.uav_id)+'/downward_cam/camera/detection_image',Image, queue_size=100, latch=True)
self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10)
self.ts.registerCallback(self.callback)
mavros.set_namespace('/uav_'+str(goal.uav_id)+'/mavros')
self.currentPoseX = -1
self.currentPoseY = -1
self.currentPoseZ = -1
self.pub = SP.get_pub_position_local(queue_size=10)
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.updatePosition)
self.image = None
self.left_image = None
self.right_image = None
self.data_small = np.genfromtxt(packagePath+'/config/small.dat', np.float32, delimiter=',')
self.data_big = np.genfromtxt(packagePath+'/config/big.dat', np.float32, delimiter=',')
self.data_dropzone = np.genfromtxt(packagePath+'/config/dropzone.dat', np.float32, delimiter=',')
self.samples = np.reshape(self.data_small, (-1, 7))
self.samples_big = np.reshape(self.data_big, (-1, 7))
self.samples_dropzone = np.reshape(self.data_dropzone, (-1, 7))
self.objects_count = 0
self.frame = 0
self.new_objects = Objects()
self.new_objects_count = 0
self.navigating = False
self.done = False
self.edgeThresholdSize = 0.1
self.width = 1600
self.height = 1200
def joy_cb(self, data):
if self.state_mach == self.DISARMED and data.buttons[20] == 1:
mavros.set_namespace()
self.set_offboard_mode()
elif self.state_mach == self.OFFBOARD and data.buttons[3] == 1:
command.arming(True)
elif self.state_mach == self.ARMED and data.buttons[4] == 1:
command.arming(False)
def __init__(self):
super().__init__()
self.mode_data = {}
mavros.set_namespace("mavros")
# register mode control
self.mode_control = rospy.ServiceProxy(mavros.get_topic("set_mode"),
SetMode)
self.mode_control_type = GraphQLObjectType(
"Mode",
lambda: {
"uuid":
GraphQLField(GraphQLString,
description="The uuid of the vehicle"),
"modeString":
GraphQLField(GraphQLString, description=""),
"baseMode":
GraphQLField(GraphQLInt, description=""),
"customMode":
GraphQLField(GraphQLInt, description=""),
"updateTime":
GraphQLField(GraphQLInt, description=""),
"returncode":
GraphQLField(GraphQLInt, description=""),
},
description="Mode control",
)
self.q = {
"Mode":
GraphQLField(
self.mode_control_type,
args={
"uuid":
GraphQLArgument(
GraphQLNonNull(GraphQLString),
description="The uuid of the target vehicle",
)
},
resolve=self.get_mode,
)
}
self.m = {
"Mode":
GraphQLField(
self.mode_control_type,
args=self.get_mutation_args(self.mode_control_type),
resolve=self.update_mode,
)
}
self.s = {
"Mode":
GraphQLField(self.mode_control_type,
subscribe=self.sub_mode,
resolve=None)
}
def init():
#rospy.init_node('mavsys', anonymous=True)
#rospy.init_node('mavsafety', anonymous=True)
#rospy.init_node('mavsetp', anonymous=True)
#rospy.init_node('mavcmd', anonymous=True)
mavros.set_namespace('/mavros')
rospy.init_node('time', anonymous=True)
return rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
def setpoint_demo():
rospy.init_node('setpoint_position_demo_1')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/uav_1/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
time.sleep(1)
rospy.loginfo("Climb")
setpoint.takeoff(1.0)
# setpoint.navigate()
rospy.loginfo("Moving to Pose 1")
#setpoint.setPose(0,0,1,5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(7, -1, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(7.862669, -8.131087, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3.296570, -8.891940, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3.413211, -6, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-4.830871, -6, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-6.704798, -6, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-6.692234, -8, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-6.362257, 7.847113, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-4.927931, 5.589492, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-2.475843, 6.678944, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3, 8.609401, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(8.157134, 8.609435, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(8.201663, 1.988020, 1, 5)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(-3, 1.942599, 1, 5)
rospy.loginfo("Landing")
setpoint.land()
rospy.loginfo("Bye!")
def __init__(self, hz=60):
rospy.loginfo("ARObstacleController Started!")
mavros.set_namespace()
self.feed_sub = rospy.Subscriber(
"/camera/rgb/image_rect_color/compressed", CompressedImage,
self.image_cb)
self.state_sub = rospy.Subscriber("/mavros/state", State,
self.state_cb)
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped, self.local_pose_cb)
self.local_pose_sp_pub = rospy.Publisher(
"/mavros/setpoint_position/local", PoseStamped, queue_size=1)
if _INTEGRATED:
self.local_vel_sp_pub = rospy.Publisher("/ar_vel",
TwistStamped,
queue_size=1)
else:
self.local_vel_sp_pub = rospy.Publisher(
"/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=1)
self.pub_error = rospy.Publisher(
"/obs_error", Twist,
queue_size=1) # set data type to publish to error
self.ar_vel = rospy.Publisher("/ar_vel", TwistStamped, queue_size=1)
self.ar_pose_sub = rospy.Subscriber("/ar_aero_pose", AlvarMarkers,
self.ar_pose_cb)
self.rate = rospy.Rate(hz)
self.current_state = State()
self.current_pose = None
self.current_vel = None
'''
0 is hovering in space,
1 is flying to obstacle
2 is ring flythru (marker id: 24)
3 is hurdle flyover (marker id: 12)
4 is gate flyunder (marker id: 9)
'''
self.finite_state = 0
self.markers = []
self.vel_hist = [[], [], [], []]
self.current_obstacle_seq = 0
self.current_obstacle_tag = None
self.t_marker_last_seen = None
self.t_obstacle_start = None
self.local_vel_sp = TwistStamped()
self.local_pose_sp = None
self.offboard_vel_streaming = False
self.tl = tf.TransformListener()
def main():
rospy.init_node('breck_field_gps_test1')
mavros.set_namespace()
# Create logger
logger = Logger('logs/breck_field_gps_test1')
# Connect to vehicle
logger.log_info('Initializing vehicle...')
vehicle = Vehicle(logger)
# Initialize setpoints
logger.log_info('Initializing setpoints...')
gps_sp = GPS_Setpoint(vehicle)
vel_sp = Velocity_Setpoint(vehicle)
# Set to takeoff
logger.log_info('Taking off!')
if not vehicle.set_mode('AUTO.TAKEOFF'):
logger.log_warning('Mode change rejected.')
vehicle.arm()
time.sleep(15)
# Navigate to target detection location
logger.log_info('Setting GPS location...')
gps_sp.set(37.897002, -86.581023, 10)
# Set back to OFFBOARD mode
logger.log_info('Entering OFFBOARD mode and navigating to GPS location...')
if not vehicle.set_mode('OFFBOARD'):
logger.log_warning('Mode change rejected.')
return
# Wait until reached
gps_sp.wait_until_reached()
logger.log_info('Location reached! Loitering for 15 seconds...')
logger.log_info('Entering LOITER mode...')
if not vehicle.set_mode('AUTO.LOITER'):
logger.log_warning('Mode change rejected.')
return
time.sleep(5)
if not vehicle.set_mode('AUTO.LAND'):
logger.log_warning('Mode change rejected.')
return
gps_sp.terminate()
vel_sp.terminate()
# Done
logger.log_info('Done!')
def __init__(self):
# Setup the nodes and the namespace
# If the drone is a slave generate the master ID
rospy.init_node("uav_pos_services", anonymous=True)
self.rate = rospy.Rate(20)
mavros.set_namespace('mavros')
# Initialize the parameters
self.local_pos = [0.0] * 4
self.global_pos = [0.0] * 4
self.UAV_state = mavros_msgs.msg.State()
# setup subscribers
# /mavros/state
state_sub = rospy.Subscriber(mavros.get_topic('state'),
mavros_msgs.msg.State,
self._state_callback)
# /mavros/local_position/pose
local_position_sub = rospy.Subscriber(
mavros.get_topic('local_position', 'pose'), SP.PoseStamped,
self._local_position_callback)
# /mavros/global_position/global
global_position_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self._global_position_callback)
# setup publisher
# /mavros/setpoint/position/local
self.setpoint_local_pub = mavros.setpoint.get_pub_position_local(
queue_size=10)
# setup the services
# /mavros/cmd/arming
self.set_arming = rospy.ServiceProxy('mavros/cmd/arming',
mavros_msgs.srv.CommandBool)
# /mavros/set_mode
self.set_mode = rospy.ServiceProxy('mavros/set_mode',
mavros_msgs.srv.SetMode)
# Setup global and local service server
sg = rospy.Service("target_global_pos", target_global_pos,
self.GotoGlobPos)
sl = rospy.Service("target_local_pos", target_local_pos,
self.GotoLocPos)
self.setpoint_msg = mavros.setpoint.PoseStamped(
header=mavros.setpoint.Header(frame_id="att_pose",
stamp=rospy.Time.now()), )
rospy.loginfo("The target services were successfully initiated")
rospy.spin()
def __init__(self):
self.local_pos = [0.0] * 4
rospy.init_node("uav_node")
self.rate = rospy.Rate(20)
mavros.set_namespace('mavros')
signal.signal(signal.SIGINT, self.signal_handler)
self.UAV_state = mavros_msgs.msg.State()
# setup subscribers
# /mavros/state
state_sub = rospy.Subscriber(mavros.get_topic('state'),
mavros_msgs.msg.State,
self._state_callback)
# /mavros/imu/data
rospy.Subscriber('/mavros/imu/data', Imu, self.IMU_callback)
# # /mavros/local_position/pose
local_position_sub = rospy.Subscriber(
mavros.get_topic('local_position', 'pose'), SP.PoseStamped,
self._local_position_callback)
# /mavros/setpoint_raw/target_local
setpoint_local_sub = rospy.Subscriber(
mavros.get_topic('setpoint_raw', 'target_local'),
mavros_msgs.msg.PositionTarget, self._setpoint_position_callback)
# setup services
# /mavros/cmd/arming
self.set_arming = rospy.ServiceProxy('mavros/cmd/arming',
mavros_msgs.srv.CommandBool)
# mavros/cmd/command
self.command_srv = rospy.ServiceProxy('mavros/cmd/command',
mavros_msgs.srv.CommandLong)
# /mavros/set_mode
self.set_mode = rospy.ServiceProxy('mavros/set_mode',
mavros_msgs.srv.SetMode)
# goto_pos_services
self.goto_loc_pos_serv = rospy.ServiceProxy("target_local_pos",
target_local_pos)
self.goto_pid_pos_serv = rospy.ServiceProxy("goto_pid_service",
goto_pid)
# aruco based services
# self.goto_aruco_serv = rospy.ServiceProxy('goto_aruco', goto_aruco)
# self.land_aruco_serv = rospy.ServiceProxy('land_aruco', land_aruco)
# wait for FCU connection
while (not self.UAV_state.connected):
self.rate.sleep()
rospy.loginfo("Uav was successfully connected")
self.InputHandler()
def setpoint_demo():
rospy.init_node('setpoint_position_demo_1')
mavros.set_namespace('/uav_1/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
time.sleep(1)
rospy.loginfo("Landing Drone 1")
setpoint.land()
rospy.loginfo("Bye!")
def __init__(self):
# Arm the drone
mavros.set_namespace()
command.arming(True)
self.pub_sp = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
rospy.init_node('gotowaypoint', anonymous=True)
rate = rospy.Rate(60) # 10hz
# Parse input
#parser = argparse.ArgumentParser()
#parser.add_argument("--boundary", help="filepath to the control boundary")
#args = parser.parse_args(rospy.myargv(argsv))
#self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.wait_for_service('mavros/set_mode')
self.change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
data = self.load_config('scripts/boundary.yaml')
self.boundary = Boundary(data)
self.controller = QUAD_position_controller(
self.load_config('scripts/gainsStart.yaml'))
# Set parameters
self.kz = 0.05
self.hoverth = 0.565
# Subscribe to local position
self.local_pose = PoseStamped()
self.velocity = TwistStamped()
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._send_command)
rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped,
self._read_velocity)
# Set Waypoint
self.waypoint = PoseStamped()
self.waypoint.pose.position.x = -1.4
self.waypoint.pose.position.y = -5
self.waypoint.pose.position.z = 3
self.marker_pub = rospy.Publisher('/waypoint_marker',
Marker,
queue_size=10)
self.waypoint_marker = Marker()
self.publish_waypoint()
# Define Controller
rospy.spin()
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/iris/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
rospy.loginfo("Initiate UAV")
setpoint.set(-13.0, -8.5, 4.0, 0)
rospy.loginfo("Cheers Mate!")
def main():
# print "TASK: "+str(sys.argv)
# setup rospy env
rospy.init_node('TCS_task', anonymous=True)
rate = rospy.Rate(20)
mavros.set_namespace('/mavros')
# setup local pub
setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10)
# setup setpoint_msg
setpoint_msg = mavros.setpoint.PoseStamped(
header=mavros.setpoint.Header(
frame_id="local_pose",
stamp=rospy.Time.now()),
)
# setup local sub
position_local_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, local_position_cb)
# setup task pub
task_watchdog = TCS_util.Task_watchdog('TASK_LOCAL_GOTO')
# interprete the input position
setpoint_arg = sys.argv[1].split(' ')
setpoint_position.x=float(setpoint_arg[0])
setpoint_position.y=float(setpoint_arg[1])
setpoint_position.z=float(setpoint_arg[2])
print "X: {}, Y: {}, Z: {}".format(setpoint_position.x,
setpoint_position.y, setpoint_position.z)
# setup setpoint poisiton and prepare to publish the position
set_target(setpoint_msg,
setpoint_position.x,
setpoint_position.y,
setpoint_position.z)
# In this while loop, do the job.
while(not is_reached()):
# When the UAV reached the position,
# publish the task finished signal and exit
setpoint_local_pub.publish(setpoint_msg)
# TODO: publish the task status as conducting
task_watchdog.report_running()
rate.sleep()
# TODO: publish the task status as FINISHING
task_watchdog.report_finish()
return 0
def main():
parser = argparse.ArgumentParser(
description="This node launch/terminate processes on events.")
parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE)
args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
rospy.init_node("event_launcher")
mavros.set_namespace(args.mavros_ns)
rospy.loginfo("Starting event launcher...")
launcher = Launcher()
launcher.spin()
def setUp(self):
rospy.init_node('test_node', anonymous=True)
#self.helper = PX4TestHelper("mavros_offboard_posctl_test")
#self.helper.setUp()
mavros.set_namespace('/uav_2/mavros')
rospy.Subscriber("/uav_2/mavros/local_position/pose", PoseStamped, self.position_callback)
rospy.Subscriber("/uav_2/mavros/global_position/global", NavSatFix, self.global_position_callback)
self.pub_spt = rospy.Publisher('/uav_2/mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.wait_for_service('/uav_2/mavros/cmd/command', 30)
self._srv_cmd_long = rospy.ServiceProxy('/uav_2/mavros/cmd/command', CommandLong, persistent=True)
self.rate = rospy.Rate(10) # 10hz
self.has_global_pos = False
self.local_position = PoseStamped()
self.armed = False
def __init__(self, copter_id = "1"):
self.copter_id = copter_id
mavros_string = "/mavros"
#rospy.init_node('velocity_goto_'+copter_id)
mavros.set_namespace(mavros_string) # initialize mavros module with default namespace
self.mavros_string = mavros_string
self.final_alt = 0.0
self.final_pos_x = 0.0
self.final_pos_y = 0.0
self.final_vel = 0.0
self.cur_rad = 0.0
self.cur_alt = 0.0
self.cur_pos_x = 0.0
self.cur_pos_y = 0.0
self.cur_vel = 0.0
self.vx = 0.0
self.vy = 0.0
self.vz = 0.0
self.avz = 0.0
self.pose_open = []
self.alt_control = True
self.override_nav = False
self.reached = True
self.done = False
self.last_sign_dist = 0.0
# for local button handling
self.click = " "
self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons)
# publisher for mavros/copter*/setpoint_position/local
self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10)
# subscriber for mavros/copter*/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp)
self.cv_bridge = CvBridge()
self.image_data = open('image_data.txt','w')
def setpoint_demo():
rospy.init_node('setpoint_position_demo_2')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/uav_2/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
time.sleep(1)
rospy.loginfo("Climb")
setpoint.takeoff(13)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(25,-25,13,5)
rospy.loginfo("Landing")
setpoint.land()
rospy.loginfo("Bye!")
def __init__(self):
self.current_pose = _vector3()
self.setpoint_pose = _vector3()
self.mode = "None"
self.arm = "None"
self.guided = "None"
self.timestamp = float(datetime.utcnow().strftime('%S.%f'))
self.conn_delay = 0.0
rospy.init_node('UAV_Monitor')
mavros.set_namespace("/mavros")
# setup local_position sub
self.local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self._local_position_callback)
self.setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'),
mavros_msgs.msg.PositionTarget, self._setpoint_position_callback)
self.state_sub = rospy.Subscriber(mavros.get_topic('state'),
mavros_msgs.msg.State, self._state_callback)
pass
def main():
yaw = 0.0
thrust = 0.0
roll = 0.0
pitch = 0.0
flag = True
rospy.init_node('setpoint_position_demo')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(10)
setpoint = SetpointPosition()
i = 0
while flag:
#flag, roll, pitch, yaw, thrust = input_key(roll, pitch, yaw, thrust)
print roll
print "here"
roll = sin(i)
setpoint.set(10, pitch, yaw, thrust)
i +=1
rospy.loginfo("Bye!")
def setpoint_demo():
rospy.init_node('explore_arena_integration_test')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/uav_3/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
time.sleep(1)
rospy.loginfo("Climb")
setpoint.takeoff(10)
rospy.loginfo("Moving to Pose 1")
setpoint.setPose(0,0,15,2)
rospy.loginfo("Moving to Pose 2")
setpoint.setPose(10,10,15,10)
rospy.loginfo("Landing")
setpoint.land()
rospy.loginfo("Bye!")
def __init__(self):
self.done = False
self.done_evt = threading.Event()
self.isExploring = False
self.progress = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.currentPoseX = 0
self.currentPoseY = 0
self.currentPoseZ = 0
self.navigating = False
mavros.set_namespace('/uav_1/mavros')
# publisher for mavros/setpoint_position/local
self.pub = SP.get_pub_position_local(queue_size=10)
# subscriber for mavros/local_position/local
self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
SP.PoseStamped, self.reached)
self.objects_map = ObjectsMap()
self.client = actionlib.SimpleActionClient('TrackingAction', TrackingAction)
#client = self.client
#client = self.actionServer.client
print "Waiting for tracking server"
self.client.wait_for_server()
self.goal = TrackingGoal()
self.goal.uav_id = 1
self.client.send_goal(self.goal)
print "Waiting for result"
self.client.wait_for_result()
print "Result:"
self.objects =self.client.get_result().tracked_objects.objects
print self.objects
try:
thread.start_new_thread(self.navigate, ())
except:
fault("Error: Unable to start thread")
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/iris/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
rospy.loginfo("climb")
setpoint.set(2.0, 0.0, 5.0, 180, 0)
setpoint.set(2.0, 0.0, 10.0, 180, 1)
setpoint.set(2.0, 0.0, 11.5, 180, 1)
rospy.loginfo("move right")
setpoint.set(2.0, 3.0, 13.5, 180, 2)
setpoint.set(2.0, 5.0, 13.5, 180, 2)
setpoint.set(4.0, 10.0, 13.5, 180, 1)
setpoint.set(4.0, 25.0, 13.5, 180, 1)
setpoint.set(4.0, 27.0, 13.5, 180, 1)
setpoint.set(4.0, 31.0, 13.5, 180, 1)
rospy.loginfo("rotate")
setpoint.set(4.0, 31.0, 13.5, 90, 8)
setpoint.set(6.5, 31.0, 10.5, 90, 8)
#setpoint.set(9.0, 31.0, 10.5, 90, 5)
setpoint.set(6.5, 31.0, 8.5, 90, 8)
setpoint.set(12.0, 25.0, 8.5, 90, 5)
setpoint.set(14.0, 25.0, 8.5, 90, 5)
#setpoint.set(16.0, 26.0, 8.5, 90, 4)
#setpoint.set(4.0, 10.0, 12.5, 90, 3)
#setpoint.set(4.0, 25.0, 12.5, 90, 3)
# Simulate a slow landing.
#setpoint.set(0.0, 0.0, 8.0, 5)
#setpoint.set(0.0, 0.0, 3.0, 5)
#setpoint.set(0.0, 0.0, 2.0, 2)
#setpoint.set(0.0, 0.0, 1.0, 2)
#setpoint.set(0.0, 0.0, 0.0, 2)
#setpoint.set(0.0, 0.0, -0.2, 2)
rospy.loginfo("Bye!")
def __init__(self, copter_id = "1"):
self.copter_id = copter_id
mavros_string = NAMESPACE_PREFIX + copter_id + "/mavros"
mavros.set_namespace(mavros_string) # initialize mavros module with default namespace
self.mavros_string = mavros_string
self.final_alt = 0.0
self.final_pos_x = 0.0
self.final_pos_y = 0.0
self.final_vel = 0.0
self.cur_rad = 0.0
self.cur_alt = 0.0
self.cur_pos_x = 0.0
self.cur_pos_y = 0.0
self.cur_vel = 0.0
self.vx = 0.0
self.vy = 0.0
self.vz = 0.0
self.pose_open = []
self.alt_control = True
self.override_nav = False
self.reached = True
self.done = False
self.last_sign_dist = 0.0
# for local button handling
self.click = " "
self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons)
# publisher for mavros/copter*/setpoint_position/local
self.pub_vel = rospy.Publisher(mavros_string + '/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
# subscriber for mavros/copter*/local_position/local
self.sub = rospy.Subscriber(mavros_string + 'local_position/local', SP.PoseStamped, self.temp)
def setpoint_demo():
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('setpoint_position_demo_1')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/uav_1/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
# time.sleep(1)
try:
print 'reem'
while(1):
key = getKey()
print 's'
#print key
if key == 't':
setpoint.takeoff(2)
if key == 'l':
setpoint.land()
if key == 'r':
setpoint.setPose(1.0,0,0)
if key =='f':
setpoint.setPose(-1.0,0,0)
if key == 'g':
setpoint.setPose(0,1.0,0)
if key =='d':
setpoint.setPose(0,-1.0,0)
if key =='y':
setpoint.setPose(0,0,0)
except:
print 'r'
#print e
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def __init__(self, copter_id="1", mavros_string="/mavros/copter1"):
#rospy.init_node('velocity_goto')
mavros.set_namespace(mavros_string) # initialize mavros module with default namespace
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.current_lat = 0.0
self.current_lon = 0.0
self.current_alt = 0.0
self.throttle_update = 0.0
self.attitude_publish = True
self.mavros_string = mavros_string
self.copter_id = copter_id
self.pid_throttle = pid_controller.PID(P = 0.8, I = 0.8, D = 0.8, Integrator_max=1, Integrator_min=0)
self.pid_throttle.setPoint(10)
self.setHome = False
self.home_lat = 0.0
self.home_lon = 0.0
self.home_alt = 0.0
self.publish = True
self.velocity_init()
def main():
rospy.init_node('default_offboard', anonymous=True)
rate = rospy.Rate(10)
mavros.set_namespace('/mavros')
# setup subscriber
# /mavros/state
state_sub = rospy.Subscriber(mavros.get_topic('state'),
mavros_msgs.msg.State, _state_callback)
# /mavros/local_position/pose
# local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
# SP.PoseStamped, _local_position_callback)
# /mavros/setpoint_raw/target_local
setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'),
mavros_msgs.msg.PositionTarget, _setpoint_position_callback)
# setup publisher
# /mavros/setpoint/position/local
setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10)
# setup service
# /mavros/cmd/arming
set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
# /mavros/set_mode
set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
setpoint_msg = mavros.setpoint.PoseStamped(
header=mavros.setpoint.Header(
frame_id="att_pose",
stamp=rospy.Time.now()),
)
# wait for FCU connection
while(not UAV_state.connected):
rate.sleep()
# create task instance
task_goto = Task_GOTO_Local(setpoint_local_pub)
task_stay = Task_Stay(setpoint_local_pub)
# initialize the setpoint
setpoint_msg.pose.position.x = 0
setpoint_msg.pose.position.y = 0
setpoint_msg.pose.position.z = 3
mavros.command.arming(True)
# send 100 setpoints before starting
for i in range(0,50):
setpoint_local_pub.publish(setpoint_msg)
rate.sleep()
set_mode(0,'OFFBOARD')
last_request = rospy.Time.now()
step = 1
task_done = False
while(True):
# print "Entered whiled loop"
if( UAV_state.mode != "OFFBOARD" and
(rospy.Time.now() - last_request > rospy.Duration(5.0))):
if( set_mode(0,'OFFBOARD').success):
print "Offboard enabled"
last_request = rospy.Time.now()
else:
if(not UAV_state.armed and
(rospy.Time.now() - last_request > rospy.Duration(5.0))):
if( mavros.command.arming(True)):
print "Vehicle armed"
last_request = rospy.Time.now()
# print "Before Task loop"
if (UAV_state.armed):
# print "Now we are in step {0}".format(step)
if (step == 1):
task_goto.goto(5,5,8.1)
task_done = task_goto.check_task()
elif (step == 2):
# stay at current position for 10 seconds
task_stay.stay_at_time(10.0)
task_done = task_stay.check_task()
elif (step == 3):
task_goto.goto(-10,0,5.1)
task_done = task_goto.check_task()
elif (step == 4):
task_goto.goto(0,0,5.1)
task_done = task_goto.check_task()
else:
while (UAV_state.mode != "AUTO.LAND"):
set_mode(0,'AUTO.LAND')
rate.sleep()
return 0
# Exit program
if (task_done):
step+=1
task_stay.reset_stay()
#setpoint_local_pub.publish(setpoint_msg)
rate.sleep()
return 0
def main():
rospy.init_node('default_offboard', anonymous=True)
rate = rospy.Rate(20)
mavros.set_namespace('/mavros')
# setup subscriber
# /mavros/state
state_sub = rospy.Subscriber(mavros.get_topic('state'),
mavros_msgs.msg.State, _state_callback)
# /mavros/local_position/pose
# local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
# SP.PoseStamped, _local_position_callback)
# /mavros/setpoint_raw/target_local
setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'),
mavros_msgs.msg.PositionTarget, _setpoint_position_callback)
# setup publisher
# /mavros/setpoint/position/local
setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10)
# setup service
# /mavros/cmd/arming
set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
# /mavros/set_mode
set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
setpoint_msg = mavros.setpoint.PoseStamped(
header=mavros.setpoint.Header(
frame_id="att_pose",
stamp=rospy.Time.now()),
)
#read task list
Task_mgr = TCS_util.Task_manager('task_list.log')
# start setpoint_update instance
setpoint_keeper = TCS_util.update_setpoint(rospy)
# wait for FCU connection
while(not UAV_state.connected):
rate.sleep()
# initialize the setpoint
setpoint_msg.pose.position.x = 0
setpoint_msg.pose.position.y = 0
setpoint_msg.pose.position.z = 3
mavros.command.arming(True)
# send 100 setpoints before starting
for i in range(0,50):
setpoint_local_pub.publish(setpoint_msg)
rate.sleep()
set_mode(0,'OFFBOARD')
last_request = rospy.Time.now()
# enter the main loop
while(True):
# print "Entered whiled loop"
if( UAV_state.mode != "OFFBOARD" and
(rospy.Time.now() - last_request > rospy.Duration(5.0))):
if( set_mode(0,'OFFBOARD').success):
print "Offboard enabled"
last_request = rospy.Time.now()
else:
if(not UAV_state.armed and
(rospy.Time.now() - last_request > rospy.Duration(5.0))):
if( mavros.command.arming(True)):
print "Vehicle armed"
last_request = rospy.Time.now()
# update setpoint to stay in offboard mode
setpoint_keeper.update()
if(Task_mgr.task_finished()):
# If the current task has been done
rospy.loginfo("Current task is finished!")
if (not Task_mgr.alldone()):
# If there are tasks left
Task_mgr.nexttask()
else:
# Current task has been done and no task left
rospy.loginfo("All tasks have been done!")
while (UAV_state.mode != "AUTO.LAND"):
set_mode(0,'AUTO.LAND')
rate.sleep()
return 0
rate.sleep()
return 0
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/iris/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
#read from a file
theFile = open("/home/randa/workspace/catkin_ws/src/aircraft_inspection/inspection/scripts/path_check.txt", "r")
rospy.loginfo("File Open")
begin= rospy.get_time() # stamp should update
rospy.loginfo("TAKEOFF 1")
#setpoint.set(4.0, -30.0, 4.0, 3.14, 3)
setpoint.set(3.0, -34.5, 5.0, 3.14, 5)
rospy.loginfo("TAKEOFF 2")
#setpoint.set(4.0, -30.0, 9.0, 3.14, 6)
setpoint.set(3.0, -34.5, 9.0, 3.14, 6)
rospy.loginfo("MOVING using data from the file")
#setpoint.set(4.0, -29.0, 9.0, -2.3562, 8)
data = theFile.readlines()
temp_vals = []
temp_valsx = []
temp_valsy = []
temp_valsz = []
temp_vals.append(9.0)
temp_valsx.append(3.0)
temp_valsy.append(-34.5)
temp_valsz.append(9.0)
index=0
for line in data:
index = index+1
poses = line.split()
print float(poses[0])
print float(poses[1])
print float(poses[2])
print float(poses[3])
setpoint.set(temp_valsx[index-1], temp_valsy[index-1], temp_valsz[index-1], float(poses[3]), 5)#12
#setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 7)#12
temp_valsx.append(float(poses[0]))
temp_valsy.append(float(poses[1]))
temp_valsz.append(float(poses[2]))
#temp_vals.append(float(poses[2]))
diffx = temp_valsx[index]-temp_valsx[index-1]
diffy = temp_valsy[index]-temp_valsy[index-1]
diffz = temp_valsz[index]-temp_valsz[index-1]
#setpoint.set(float(poses[0]), float(poses[1]), temp_vals[index-1], float(poses[3]), 10)
print diffz
if diffz>0: # and temp_vals[index]>1.5:
print "diff>1"
startx = temp_valsx[index-1]
starty = temp_valsy[index-1]
startz = temp_valsz[index-1]
endz = float(poses[2])
stepz = 0.125
stepx = 0.125
stepy = 0.125
if diffx<0:
stepx=stepx*-1
elif diffx==0:
stepx=0
if diffy<0:
stepy=stepy*-1
elif diffy==0:
stepy=0
while startz < endz: # and temp_vals[index]>1.5:
setpoint.set(startx, starty, startz, float(poses[3]), 1.2)
startz += stepz
startx += stepx
starty += stepy
elif diffz<0: # and temp_vals[index]>1.5:
print "diff<-1"
startx = temp_valsx[index-1]
starty = temp_valsy[index-1]
startz = temp_valsz[index-1]
endz = float(poses[2])
stepz = 0.125
stepx = 0.125
stepy = 0.125
if diffx<0:
stepx=stepx*-1
elif diffx==0:
stepx=0
if diffy<0:
stepy=stepy*-1
elif diffy==0:
stepy=0
while startz > endz:
setpoint.set(startx, starty, startz, float(poses[3]), 1.2)
startz -= stepz
startx += stepx
starty += stepy
else:
print "nothing"
#if temp_vals[index]>1.5:
setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 5)#12
setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 5)#12
######testing###
#setpoint.set(4.0, -29.0, 9.0, -2.3562, 8)
#setpoint.set(4.0, -29.0, 9.0, 2.3562, 8)
#rospy.loginfo("MOVING 3")
#setpoint.set(6.0, -29.0, 7.0, 3.14159, 8)
#setpoint.set(6.0, -29.0, 5.0, 2.3562, 8)
end= rospy.get_time() # stamp should update
elapsed =end - begin
rospy.loginfo("elapsed time: %d s",elapsed)
rospy.loginfo("DONE")
theFile.close()
#Simulate a slow landing.
#setpoint.set(4.0, -28.0, 3.0, 180, 5)
#setpoint.set(4.0, -28.0, 2.0, 180, 2)
#setpoint.set(4.0, -28.0, 1.0, 180, 2)
#setpoint.set(4.0, -28.0, 0.0, 180, 2)
#setpoint.set(4.0, -28.0, -0.2, 180, 2)
rospy.loginfo("Bye!")
def __init__(self, copter_id = "1", mavros_string="/mavros/copter1"):
rospy.init_node('rc_override'+copter_id)
mavros.set_namespace(mavros_string) # initialize mavros module with default namespace
self.rc = OverrideRCIn()
self.override_pub = rospy.Publisher(mavros_string+"/rc/override", OverrideRCIn, queue_size=10)
import sys
import time
import rospy
import thread
import threading
import mavros
from geometry_msgs.msg import PoseStamped, Quaternion
from math import *
from std_msgs.msg import Header
from std_msgs.msg import String
from mavros.utils import *
from mavros import setpoint as SP
from tf.transformations import quaternion_from_euler
mavros.set_namespace()
class Setpoint:
def __init__(self, pub, rospy):
self.pub = pub
self.rospy = rospy
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.curr_x = 0.0
self.curr_y = 0.0
self.curr_z = 0.0
self.yaw = 0.0
#from matplotlib import pyplot as plt
from geometry_msgs.msg import PoseStamped, Quaternion
from math import *
from std_msgs.msg import Header
from std_msgs.msg import String
from mavros.utils import *
from mavros import setpoint as SP
from tf.transformations import quaternion_from_euler
from threading import Thread, Event
#from Stereo_Vision import Stereo_Vision
from MAVROS_navigation import Setpoint
#from Usonic import usonic
mavros.set_namespace()
#initialize sonar sensor network and vision system
#stereo = Stereo_Vision(cam_L_src=0,cam_R_src=1)
#usonic = usonic(stuff)
#initailize ROS publisher for setpoints at 10Hz
rospy.init_node('setpoint_position_demo')
mavros.set_namespace() # initialize mavros module with default namespace
rate = rospy.Rate(10)
setpoint = Setpoint(sonar_readings=[500,500,500,500,500,500], jMAVSim = True)
raw_input("Press any key to begin")
setpoint.altitude_change(515)
def setpoint_demo():
rospy.init_node('setpoint_position_demo')
#mavros.set_namespace() # initialize mavros module with default namespace
mavros.set_namespace('/iris/mavros')
rate = rospy.Rate(10)
setpoint = SetpointPosition()
#read from a file
theFile = open("/home/randa/workspace/catkin_ws/src/aircraft_inspection/inspection/scripts/path_check.txt", "r")
rospy.loginfo("File Open")
begin= rospy.get_time() # stamp should update
rospy.loginfo("TAKEOFF 1")
setpoint.set(4.5, 24.0, 5.0, 3.14, 5)
rospy.loginfo("TAKEOFF 2")
setpoint.set(4.5, 24.0, 7.0, 3.14, 6)
rospy.loginfo("TAKEOFF 4")
setpoint.set(4.5, 24.0, 7.0, 2.3562, 6)
rospy.loginfo("MOVING using data from the file")
#setpoint.set(4.0, -29.0, 9.0, -2.3562, 8)
data = theFile.readlines()
temp_vals = []
temp_vals.append(7.0)
index=0
for line in data:
index = index+1
poses = line.split()
print float(poses[0])
print float(poses[1])
print float(poses[2])
print float(poses[3])
temp_vals.append(float(poses[2]))
diff = temp_vals[index]-temp_vals[index-1]
setpoint.set(float(poses[0]), float(poses[1]), temp_vals[index-1], float(poses[3]), 10)
print diff
if diff>1.5: # and temp_vals[index]>1.5:
print "diff>1"
start = temp_vals[index-1]
end = float(poses[2])
step = 1.5
while start < end: # and temp_vals[index]>1.5:
start += step
setpoint.set(float(poses[0]), float(poses[1]), start, float(poses[3]), 4)
elif diff<-1.5: # and temp_vals[index]>1.5:
print "diff<-1"
start = temp_vals[index-1]
end = float(poses[2])
step = 1.5
while start > end:
start -= step
setpoint.set(float(poses[0]), float(poses[1]), start, float(poses[3]), 4)
else:
print "nothing"
#if temp_vals[index]>1.5:
setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 4)#12
######testing###
#setpoint.set(4.0, -29.0, 9.0, -2.3562, 8)
#setpoint.set(4.0, -29.0, 9.0, 2.3562, 8)
#rospy.loginfo("MOVING 3")
#setpoint.set(6.0, -29.0, 7.0, 3.14159, 8)
#setpoint.set(6.0, -29.0, 5.0, 2.3562, 8)
end= rospy.get_time() # stamp should update
elapsed =end - begin
rospy.loginfo("elapsed time: %d s",elapsed)
rospy.loginfo("DONE")
theFile.close()
#Simulate a slow landing.
#setpoint.set(4.0, -28.0, 3.0, 180, 5)
#setpoint.set(4.0, -28.0, 2.0, 180, 2)
#setpoint.set(4.0, -28.0, 1.0, 180, 2)
#setpoint.set(4.0, -28.0, 0.0, 180, 2)
#setpoint.set(4.0, -28.0, -0.2, 180, 2)
rospy.loginfo("Bye!")
