def home_publisher(pub):
data = HomePosition()
rate = rospy.Rate(100)
while not rospy.is_shutdown():
t = math.modf(time.time())
data.header.stamp.secs = int(t[1])
data.header.stamp.nsecs = int(t[0] * 10**9)
data.geo.latitude = 39.455
data.geo.longitude = 116.245
data.geo.altitude = 0
data.position.x = 0
data.position.y = 0
data.position.z = 0
data.orientation.x = 0
data.orientation.y = 0
data.orientation.z = 0
data.orientation.w = 1
data.approach.x = 0
data.approach.y = 0
data.approach.z = 0
pub.publish(data)
rate.sleep()
def setUp(self):
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', '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.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
def __init__(self, namespace, waypoints, update_frequency=10.):
"""
A Class that interfaces with MAVROS and Bladebug Controller for executing actions
"""
self.namespace = namespace['name']
self.fuel_rate_mean = 1.0
self.fuel_rate_std = 1.0
self.low_fuel = False
self.fuel = self.INIT_FUEL
self.set_battery(namespace['max_fuel'], namespace['min_fuel'],
namespace['fuel_rate'])
self._cancel_action = False
self.external_intervened = False
self.waypoints = [None]
self.home = HomePosition()
self.global_pose = NavSatFix()
self.heading = 0.0
self._current_wp = -1
self._radius = 2e-06
self._rate = rospy.Rate(update_frequency)
# Subscribers
rospy.Subscriber('/%s/mavros/global_position/compass_hdg' %
self.namespace,
Float64,
self._heading_cb,
queue_size=1)
rospy.Subscriber('/%s/mavros/home_position/home' % self.namespace,
HomePosition,
self._home_cb,
queue_size=1)
rospy.Subscriber('/%s/mavros/battery' % self.namespace,
BatteryState,
self._battery_cb,
queue_size=1)
rospy.Subscriber('/%s/mavros/global_position/global' % self.namespace,
NavSatFix,
self._global_pose_cb,
queue_size=1)
# Publisher
self._setpoint_pub = rospy.Publisher('/joy', Joy, queue_size=3)
# Service proxies
rospy.loginfo('Waiting for /%s/mavros/cmd/set_home ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/set_home' % self.namespace)
self._set_home_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/set_home' % self.namespace, CommandHome)
self.set_current_location_as_home()
# Adding initial waypoints' configuration
while self.waypoints[0] is None:
self._rate.sleep()
self.waypoints = self.waypoints + waypoints
if "simulation" in rospy.get_param("~scenario_type", "simulation"):
self.embrace_pose()
else:
self.olam = OLAM(self.namespace, update_frequency)
# Auto call functions
rospy.Timer(10 * self._rate.sleep_dur, self.intervene_observer)
def __init__(self):
self.pose = PoseStamped()
self.pose.pose.position.x = 0
self.pose.pose.position.y = 0
self.pose.pose.position.z = 1
self.current_state = State()
self.current_odometry = Odometry()
self.home_position = HomePosition()
self.home_position.position.x = 1
self.home_position.position.y = 1
self.home_position.position.z = 0
self.waypoint_x = 0
self.waypoint_y = 0
self.waypoint_z = 1
self.sequence = 0
self.vel = Twist()
def global_cb(self, data):
if self.is_published:
return
print("HomePositionInit: Global position recieved")
homeMsg = HomePosition()
homeMsg.header.stamp = rospy.Time.now()
if self.set_fixed:
homeMsg.geo.latitude = self.home_lat
homeMsg.geo.longitude = self.home_lon
homeMsg.geo.altitude = self.home_alt
else:
homeMsg.geo.latitude = data.latitude
homeMsg.geo.longitude = data.longitude
homeMsg.geo.altitude = data.altitude
self.home_pub.publish(homeMsg)
self.is_published = True
def __init__(self, connection, uav=None, update_frequency=10.):
"""
Class for Pre-flight check interface
"""
self.uav = uav
self.gps_raw = dict()
self.waypoints = list()
self.gps_text = None
self.rel_alt_text = None
self.telemetry_text = None
self.uav_home = HomePosition()
self.uav_conn = mavutil.mavlink_connection(connection)
self.geoid_interpolator = GeoidKarney(
roslib.packages.get_pkg_dir('uav_rosplan') + '/config/egm96-5.pgm')
self._radio_status_sub = rospy.Subscriber('/mavros/radio_status',
RadioStatus,
self._radio_cb,
queue_size=10)
self._waypoints_sub = rospy.Subscriber('/mavros/mission/waypoints',
WaypointList,
self._wp_cb,
queue_size=10)
self._home_sub = rospy.Subscriber('/mavros/home_position/home',
HomePosition,
self.home_cb,
queue_size=10)
self._rel_alt_sub = rospy.Subscriber('/mavros/global_position/rel_alt',
Float64,
self._relative_alt_cb,
queue_size=10)
self._gps_timer = rospy.Timer(rospy.Duration(1. / update_frequency),
self.update_gps)
# interface
self._main_window = Tk()
self._main_window.wm_title('Pre-Flight Checklist V2.10')
self._create_upper_frame()
self._create_middle_frame()
self._create_lower_frame()
self._create_param_frame()
self._create_submit_frame()
self._main_window.protocol("WM_DELETE_WINDOW", self.on_closing)
self._main_window.mainloop()
def __init__(self):
self.gps_now = NavSatFix()
self.gps_home = HomePosition()
self.pose = PoseStamped()
self.num_wp = 0
self.curr_wp = None
self.armed = False
self.curr_mode = None
rospy.wait_for_service('/mavros/set_mode')
rospy.wait_for_service('/mavros/cmd/arming')
rospy.wait_for_service('/mavros/cmd/takeoff')
rospy.wait_for_service('/mavros/cmd/set_home')
rospy.wait_for_service('/mavros/mission/push')
rospy.wait_for_service('/mavros/set_stream_rate')
#set up topic subscribers
rospy.Subscriber('/mavros/global_position/raw/fix', NavSatFix,
self.gps_cb)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self.pose_cb)
rospy.Subscriber('/mavros/home_position/home', HomePosition,
self.home_cb)
rospy.Subscriber('/mavros/mission/reached', WaypointReached,
self.wp_cb)
rospy.Subscriber('/mavros/battery', BatteryState, self.battery_cb)
rospy.Subscriber('/mavros/state', State, self.state_cb)
# set target position publisher object
self.set_pos = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
# set up service proxies
self.stream = rospy.ServiceProxy('/mavros/set_stream_rate', StreamRate)
self.set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
self.takeoff = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)
self.arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.send_wp = rospy.ServiceProxy('/mavros/mission/push', WaypointPush)
self.set_home = rospy.ServiceProxy('/mavros/cmd/set_home', CommandHome)
# set stream using the service
self.set_stream()
rospy.on_shutdown(self.land)
rospy.loginfo('Drone initialized ...')
def __init__(self):
self.state = State()
self.altitude = Altitude()
self.global_position = HomePosition()
self.local_position = PoseStamped() #fcu local position
self.local_position_odom = Odometry()
self.extended_state = ExtendedState()
self.mode = ''
self.rate = rospy.Rate(10)
self.service_timeout = 30
self.offboard_request_threshold = rospy.Duration(5.0)
self.arming_request_threshold = rospy.Duration(5.0)
self.imu = Imu()
self.current_yaw = None
self.setup_pubsub()
self.setup_services()
self.current_target_pose = PositionTarget()
self.current_pose = Point()
self.home_pose = Point(0.0,0.0,0.0)
self.waypoint_1 = Point(0.0,0.0,3.0)
self.should_start_mission = False
def __init__(self, *args):
super(MavrosQuad, self).__init__(*args)
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.local_velocity = TwistStamped()
self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link'
self.gazebo_load_pose = Pose()
self.gazebo_load_twist = Twist()
self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0'
self.gazebo_quad_pose = Pose()
self.gazebo_quad_twist = Twist()
self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link'
self.gazebo_imu_pose = Pose()
self.gazebo_imu_twist = Twist()
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos',
'local_pos', 'local_vel', 'mission_wp', 'state', 'imu'
]
}
# 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:
rospy.logerr("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.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state',
State,
self.state_callback)
self.local_vel_sub = rospy.Subscriber('mavros/local_position/velocity_local',
TwistStamped,
self.local_velocity_callback)
self.imu_sub = rospy.Subscriber('/mavros/imu/data',
Imu,
self.imu_callback)
self.gazebo_sub = rospy.Subscriber('/gazebo/link_states',
LinkStates,
self.gazebo_callback)
quad_obj = vicon_quad()
r = rospy.Rate(50) # 50hz
# Subscribers
vicon_sub = rospy.Subscriber('/mavros/local_position/pose',
PoseStamped,
vicon_callback,
queue_size=10)
#vicon_sub = rospy.Subscriber('/vicon/QuadrotorMark/QuadrotorMark',
# TransformStamped, vicon_callback, queue_size=10)
# Publishers
pose_pub = rospy.Publisher('/mavros/home_position/home',
HomePosition,
queue_size=1)
p = HomePosition()
setpnt_pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
quad_pose = PoseStamped()
quad_pose.pose.position.x = 0
quad_pose.pose.position.y = 0
quad_pose.pose.position.z = 0
for i in range(50):
setpnt_pub.publish(quad_pose)
r.sleep()
while not rospy.is_shutdown():
pass
def __init__(self, *args):
super(MavrosQuad, self).__init__(*args)
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.local_velocity = TwistStamped()
self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link'
self.gazebo_load_pose = Pose()
self.gazebo_load_twist = Twist()
self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0'
self.gazebo_quad_pose = Pose()
self.gazebo_quad_twist = Twist()
self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link'
self.gazebo_imu_pose = Pose()
self.gazebo_imu_twist = Twist()
self.q_input = np.array([0, 0, 0, 1])
self.T_input = 0
self.offboard_position_active = True
self.offboard_attitude_active = False
self.offboard_load_active = False
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos',
'local_pos', 'local_vel', 'mission_wp', 'state', 'imu'
]
}
# 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:
rospy.logerr("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.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.local_vel_sub = rospy.Subscriber(
'mavros/local_position/velocity_local', TwistStamped,
self.local_velocity_callback)
self.imu_sub = rospy.Subscriber('/mavros/imu/data', Imu,
self.imu_callback)
self.gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates,
self.gazebo_callback)
# ROS Publishers
# Attitude
self.att = AttitudeTarget()
self.att_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.att_thread = Thread(target=self.send_att, args=())
self.att_thread.daemon = True
self.att_thread.start()
# Pose
self.pos = PoseStamped()
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()
def __init__(self,
namespace,
waypoints,
uavs=list(),
update_frequency=10.):
"""
A Class that interfaces with MAVROS for executing actions
"""
self.wp_reached = -1
self.current_mode = ''
self.previous_mode = ''
self.namespace = namespace
self._cancel_action = False
self.external_intervened = False
self.state = State()
self.waypoints = list()
self.home = HomePosition()
self.global_pose = NavSatFix()
self.heading = 0.0
self.fuel = self.INIT_FUEL
self.low_fuel = False
self._waypoints = waypoints
self._current_wp = -1
self._status_text = ''
self._rate = rospy.Rate(update_frequency)
# Service proxies
rospy.loginfo('Waiting for service /%s/mavros/mission/push ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/mission/push' % self.namespace)
self._add_wp_proxy = rospy.ServiceProxy(
'/%s/mavros/mission/push' % self.namespace, WaypointPush)
rospy.loginfo(
'Waiting for service /%s/mavros/mission/set_current ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/mission/set_current' %
self.namespace)
self._set_current_wp_proxy = rospy.ServiceProxy(
'/%s/mavros/mission/set_current' % self.namespace,
WaypointSetCurrent)
rospy.loginfo('Waiting for /%s/mavros/set_mode ...' % self.namespace)
rospy.wait_for_service('/%s/mavros/set_mode' % self.namespace)
self._set_mode_proxy = rospy.ServiceProxy(
'/%s/mavros/set_mode' % self.namespace, SetMode)
rospy.loginfo('Wait for service /%s/mavros/mission/clear ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/mission/clear' % self.namespace)
self._clear_wp_proxy = rospy.ServiceProxy(
'/%s/mavros/mission/clear' % self.namespace, WaypointClear)
rospy.loginfo('Waiting for /%s/mavros/cmd/arming ...' % self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/arming' % self.namespace)
self._arming_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/arming' % self.namespace, CommandBool)
rospy.loginfo('Waiting for /%s/mavros/cmd/set_home ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/set_home' % self.namespace)
self._set_home_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/set_home' % self.namespace, CommandHome)
# UAV service proxies for updating UAV home position
if len(uavs):
self._uav_home_proxies = {
uav:
rospy.ServiceProxy('/%s/mavros/cmd/set_home' % uav.namespace,
CommandHome)
for uav in uavs
}
self.uav_home_wp = {uav: HomePosition() for uav in uavs}
self._uav_home_dist = {uav: float('inf') for uav in uavs}
for uav in uavs:
rospy.Subscriber('/%s/mavros/home_position/home' %
uav.namespace,
HomePosition,
lambda i: self._uav_home_cb(i, uav),
queue_size=10)
rospy.Timer(20 * self._rate.sleep_dur, self.update_uav_home_pos)
# Subscribers
rospy.Subscriber('/%s/mavros/state' % self.namespace,
State,
self._state_cb,
queue_size=10)
# halt until mavros is connected to a asv
rospy.loginfo('Waiting for a connection between MAVROS and ASV ...')
while (not self.state.connected):
self._rate.sleep()
rospy.Subscriber('/%s/mavros/global_position/compass_hdg' %
self.namespace,
Float64,
self._heading_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/home_position/home' % self.namespace,
HomePosition,
self._home_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/battery' % self.namespace,
BatteryState,
self._battery_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/global_position/global' % self.namespace,
NavSatFix,
self._global_pose_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/mission/reached' % self.namespace,
WaypointReached,
self._wp_reached_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/statustext/recv' % self.namespace,
StatusText,
self._status_text_cb,
queue_size=10)
# Auto call functions
rospy.Timer(10 * self._rate.sleep_dur, self.intervene_observer)
rospy.Timer(self._rate.sleep_dur, self.update_wp_position)
rospy.loginfo('Adding WPs ...')
# change mode just to fill self.current_mode and self.previous_mode
self.guided_mode()
# Adding initial waypoints' configuration
while not self.add_waypoints():
self._rate.sleep()
def __init__(self,
ros_rate=10, # slow as nothing happens in the main loop
state_estimation_mode=State_estimation_method.GPS,
enforce_height_mode_flag=False,
height_mode_req=0,
):
self.sem = state_estimation_mode
self._node_alive = True
self.ros_rate = ros_rate
self.wd_initialised = False
self.wd_fault_detected = False
self.fault_this_loop = False
# initialise data containers
self.altitude = Altitude()
self.altitude_bottom_clearance = Float32()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.optic_flow_raw = OpticalFlowRad()
self.optic_flow_range = Range()
self.home_position = HomePosition()
self.local_position = PoseStamped()
# self.gt_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mocap_pose = PoseStamped()
self.camera_pose = PoseStamped()
self.camera_yaw = None
self.local_x = Float64().data
self.local_y = Float64().data
self.local_z = Float64().data
self.gt_x = Float64().data
self.gt_y = Float64().data
self.gt_z = Float64().data
# todo - we can probably live with just XX_vel_bod data?:
self.x_vel = Float64().data
self.y_vel = Float64().data
self.gt_x_vel = Float64().data
self.gt_y_vel = Float64().data
self.vel_ts = Float64().data
self.xy_vel = Float64().data
self.x_vel_bod = Float64().data
self.y_vel_bod = Float64().data
self.xy_vel_bod = Float64().data
self.body_yaw_rate = Float64().data
self.global_compass_hdg_deg = Float64().data
self.enforce_height_mode_flag = enforce_height_mode_flag
self.height_mode_req = height_mode_req
# threading locks
self.setpoint_lock = Lock() # used for setting lock in our setpoint publisher so that commands aren't mixed
# ROS services
service_timeout = 10
rospy.loginfo("Searching for mavros services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/param/set', 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.wait_for_service('mavros/set_mode', service_timeout)
rospy.wait_for_service('/mavros/cmd/set_home')
# rospy.wait_for_service('mavros/fcu_url', service_timeout) # todo - check how this is used in px4
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_param_srv = rospy.ServiceProxy('mavros/param/set', ParamSet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush)
self.takeoff_srv = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)
self.cmd_home_srv = rospy.ServiceProxy('/mavros/cmd/set_home', CommandHome)
rospy.loginfo("Required ROS services are up")
except rospy.ROSException:
self.shut_node_down(extended_msg="failed to connect to Mavros services - was the mavros node started?")
# make sure we have information about our connection with FCU
rospy.loginfo("Get our fcu string")
try:
self.fcu_url = rospy.get_param('mavros/fcu_url')
except Exception as e:
print (e)
self.shut_node_down(extended_msg="cant find fcu url")
# ensure that our height mode is as we expect it to be (if required)
rospy.loginfo('check height_mode {}'.format(self.enforce_height_mode_flag))
if self.enforce_height_mode_flag:
# todo - allow multiple attempts at this
# res = self.mavros_interface.get_param_srv(param)
param_read_attempts = 0
try:
while param_read_attempts < 5:
res = self.get_param_srv('EKF2_HGT_MODE')
if res.success:
self.height_mode = res.value.integer
if self.height_mode == self.height_mode_req:
rospy.loginfo('height mode {} as expected'.format(self.height_mode))
break
else:
raise Exception ("height mode is {} - (expected heightmode is {}) change parameter with QGround control and try again".format(self.height_mode, self.height_mode_req))
break
else:
rospy.logerr( "Couldn't read EKF2_HGT_MODE param on attempt {} - trying again".format(param_read_attempts))
param_read_attempts += 1
rospy.sleep(2)
except Exception as e:
rospy.logerr(
"Couldn't read EKF2_HGT_MODE - shutting down".format(param_read_attempts))
self.shut_node_down(extended_msg= "height_mode error - traceback is {}".format(e))
# todo: ensure that our state estimation parameters are as available (this requires the state estimation
# topic name so can't test this until we do something with mocap again) Actually, we can incorporate this into
# the watchdog
# if state_estimation_mode == State_estimation_method.MOCAP:
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',ExtendedState,self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',NavSatFix, self.global_position_callback)
self.optic_flow_raw_sub = rospy.Subscriber('mavros/px4flow/raw/optical_flow_raw',OpticalFlowRad, self.optic_flow_raw_callback)
self.optic_flow_range_sub = rospy.Subscriber('mavros/px4flow/ground_distance',Range,self.optic_flow_range_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback)
self.mocap_pos_sub = rospy.Subscriber('mavros/vision_pose/pose', PoseStamped, self.mocap_pos_callback)
# self.camera_pose_sub = rospy.Subscriber(self.camera_pose_topic_name, PoseStamped, self.cam_pose_cb)
# todo - add check for this signal to watchdog - or remap /mavros/local_position/velocity -> /mavros/local_position/velocity_local
self.velocity_local_sub = rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.vel_callback)
self.velocity_body_sub = rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.vel_bod_callback)
self.compass_sub = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, self.compass_hdg_callback)
self.ground_truth_sub = rospy.Subscriber('/body_ground_truth', Odometry, self.gt_position_callback)
## Ros publishers
self.local_pos_pub_raw = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
# ROS topics - this must come after our ROS subscribers
topics_timeout = 30
rospy.loginfo("waiting for ROS topics")
try:
# check that essential messages are being subscribed to regularly
for _ in np.arange(2):
rospy.wait_for_message('mavros/local_position/pose', PoseStamped, topics_timeout)
rospy.wait_for_message('mavros/extended_state', ExtendedState, topics_timeout)
except rospy.ROSException:
self.shut_node_down(extended_msg="Required ros topics not published")
rospy.loginfo("ROS topics are up")
# create a watchdog thread that checks topics are being received at the expected rates
self.watchdog_thread = Thread(target=self.watchdog, args=())
self.watchdog_thread.daemon = True
self.watchdog_thread.start()
def talker():
car_pos_pub = rospy.Publisher("mavros_ruying/local_position/pose", PoseStamped, queue_size=10)
car_vel_pub = rospy.Publisher("mavros_ruying/local_position/velocity_local", TwistStamped, queue_size=10)
pos_image_pub = rospy.Publisher("tracker/pos_image", Float32MultiArray, queue_size=10)
car_home_pub = rospy.Publisher("mavros_ruying/home_position/home", HomePosition, queue_size=10)
rospy.init_node('pub_node', anonymous=True)
interval_rate = 50
interval_time = 1.0 / interval_rate
rate = rospy.Rate(interval_rate)
car_pos = PoseStamped()
car_vel = TwistStamped()
pos_image = Float32MultiArray()
car_yaw = np.pi/3
car_vel.twist.linear.y = 0
car_vel.twist.angular.z = 0
car_pos.pose.position.x = 1.2
car_pos.pose.position.z = 2
car_pos.pose.orientation.w = np.cos(car_yaw/2)
car_pos.pose.orientation.x = 0
car_pos.pose.orientation.y = 0
car_pos.pose.orientation.z = np.sin(car_yaw/2)
pos_image.data = [0, 0, 0, 0, 0]
mav_home = HomePosition()
mav_home.geo.latitude = 47.3977422
mav_home.geo.longitude = 8.5455935
mav_home.geo.altitude = 535.3059164018949
car_home = HomePosition()
car_home.geo = mav_home.geo
# car_home.position.x = car_pos.pose.position.x
# car_home.position.y = car_pos.pose.position.y
# car_home.position.z = car_pos.pose.position.z
car_home.orientation.w = car_pos.pose.orientation.w
car_home.orientation.x = car_pos.pose.orientation.x
car_home.orientation.y = car_pos.pose.orientation.y
car_home.orientation.z = car_pos.pose.orientation.z
dlt_home_pos = u.GeoToENU([mav_home.geo.latitude, mav_home.geo.longitude, mav_home.geo.altitude], [car_home.geo.latitude, car_home.geo.longitude, car_home.geo.altitude])
print("dlt_home_pos: {}".format(dlt_home_pos))
# car_pos.pose.position.x -= dlt_home_pos[1]
# car_pos.pose.position.y -= dlt_home_pos[0]
# car_pos.pose.position.z -= dlt_home_pos[2]
cnt = 0
while not rospy.is_shutdown():
if cnt < 500:
car_vel.twist.linear.x = 0
elif cnt < 6000:
car_vel.twist.linear.x = 1
elif cnt < 6200:
car_vel.twist.linear.x = 0
car_yaw = -2*np.pi/3
else:
car_vel.twist.linear.x = -1
#计算距离
car_pos.pose.position.x += car_vel.twist.linear.x * interval_time
car_pos.pose.position.y += car_vel.twist.linear.y * interval_time
car_pos.pose.position.z += car_vel.twist.linear.z * interval_time
car_yaw += car_vel.twist.angular.z * interval_time
# car_yaw = 0.2*np.random.rand(1)[0]-0.1
car_pos.pose.orientation.w = np.cos(car_yaw/2)
car_pos.pose.orientation.x = 0
car_pos.pose.orientation.y = 0
car_pos.pose.orientation.z = np.sin(car_yaw/2)
car_pos_pub.publish(car_pos)
car_vel_pub.publish(car_vel)
if SIM_MODE == "JMAVSIM":
pos_image_pub.publish(pos_image)
if cnt % 100 == 0:
car_home_pub.publish(car_home)
cnt += 1
rate.sleep()
def __init__(self):
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.imu_data = Imu()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.pos = PoseStamped()
self.radius = 1
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state', 'imu'
]
}
# 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.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.imu_data_sub = rospy.Subscriber('mavros/imu/data', Imu,
self.imu_data_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
# ROS publisher
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=1)
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
class bt_missions:
rospy.init_node('mavros_takeoff_python')
mavros.set_namespace()
rate = rospy.Rate(20)
dirct_ = Float64()
home_ = HomePosition()
current_state = State()
current_ = NavSatFix()
alt_ = Float64()
img = Image()
fourcc = cv2.VideoWriter_fourcc('X', 'V', 'I', 'D')
out = cv2.VideoWriter('realsense.avi', fourcc, 20.0, (640, 480))
out1 = cv2.VideoWriter('usb_cam.avi', fourcc, 20.0, (640, 480), 0)
home_set_ = False
isContinue = True
dist_first = True
kp_count = True
ah_flag = False
forward_flag = False
target = [[24.982550663278158, 121.57233949235275], [24.981975, 121.57176],
[24.981758, 121.571963]]
forward_t = 0
correction_t = 0
index = 0
time_index = 0
p0 = 0
p1 = 0
brng = 0
dLon = 0
y = 0
x = 0
current_yaw = 0
current_alt = 0
dist = 0
heading = 0
lat = 0
lon = 0
cnt = 0
img_count = 0
query_count = 0
monoq_count = 0
record_img = np.array([])
record_mono = np.array([])
set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
set_v_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position',
'local'),
PoseStamped,
queue_size=10)
def __init__(self):
self.hdg_subscriber = rospy.Subscriber(
"/mavros/global_position/compass_hdg", Float64, self.compass)
self.rel_alt = rospy.Subscriber("/mavros/global_position/rel_alt",
Float64, self.altitude)
self.home_sub = rospy.Subscriber("/mavros/home_position/home",
HomePosition, self.setHomeGeoPointCB)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global",
NavSatFix, self.current_position_cb)
self.state_sub = rospy.Subscriber(mavros.get_topic('state'), State,
self.state_cb)
self.img_sub = rospy.Subscriber("/camera/rgb/image_raw",
Image,
self.call,
buff_size=2**24,
queue_size=1)
self.mono_sub = rospy.Subscriber("/camera/mono/image_raw",
Image,
self.mono_cb,
buff_size=2**24,
queue_size=1)
self.tree = (self.achieve | (
(self.achieve_alt | self.up) >>
(self.yaw_correction | self.correction) >> self.forward
)) >> (
self.ah | (self.isdetect >> self.change_alt)
) #>> (self.realsense_check | self.change_alt | (self.mono_detect >> self.change_alt))
def compass(self, dirct):
bt_missions.dirct_ = dirct
bt_missions.heading = bt_missions.dirct_.data
def state_cb(self, state):
bt_missions.current_state = State()
bt_missions.current_state = state
def altitude(self, alt):
bt_missions.alt_ = alt
bt_missions.current_alt = bt_missions.alt_.data
def setHomeGeoPointCB(self, home):
bt_missions.home_ = home
bt_missions.home_set_ = True
rospy.loginfo(
"Received Home (WGS84 datum): %lf, %lf, %lf" %
(bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude,
bt_missions.home_.geo.altitude))
def current_position_cb(self, current):
bt_missions.current_ = current
bt_missions.lat = bt_missions.current_.latitude
bt_missions.lon = bt_missions.current_.longitude
def call(self, callimage):
bt_missions.record_img = CvBridge().imgmsg_to_cv2(callimage, "bgr8")
bt_missions.out.write(bt_missions.record_img)
def mono_cb(self, monoimage):
bt_missions.record_mono = CvBridge().imgmsg_to_cv2(monoimage, "mono8")
bt_missions.out1.write(bt_missions.record_mono)
@condition
def ah(self):
if bt_missions.ah_flag == True:
return False
else:
return True
'''@condition
def realsense_check(self):
if bt_missions.forward_flag == True:
return False
else:
return True'''
@condition
def isdetect(self):
bt_missions.query_count = bt_missions.query_count + 1
img1 = cv2.imread('query/' + str(bt_missions.query_count) + '.PNG',
cv2.IMREAD_GRAYSCALE) #queryImage
bt_missions.img_count = bt_missions.img_count + 1
cv2.imwrite('image/' + str(bt_missions.img_count) + '.PNG',
bt_missions.record_img)
img2 = cv2.imread('image/' + str(bt_missions.img_count) + '.PNG',
cv2.IMREAD_GRAYSCALE) #trainImage
# Initiate SIFT detector
sift = cv2.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
# BFMatcher with default params
bf = cv2.BFMatcher()
t = rospy.get_time()
while (rospy.get_time() - t <= 2):
matches = bf.knnMatch(des1, des2, k=2)
# Apply ratio test
good = []
for m, n in matches:
if m.distance < 0.55 * n.distance:
good.append([m])
print("threshold: %s" % len(good))
if len(good) >= 10:
bt_missions.index = bt_missions.index + 1
bt_missions.img_count = 0
bt_missions.kp_count = False
return True
else:
bt_missions.forward_flag = True
return False
@condition
def mono_detect(self):
bt_missions.monoq_count = bt_missions.monoq_count + 1
img1 = cv2.imread('query/' + str(bt_missions.monoq_count) + '.PNG',
cv2.IMREAD_GRAYSCALE) #queryImage
bt_missions.img_count = bt_missions.img_count + 1
cv2.imwrite('image/' + str(bt_missions.img_count) + '.PNG',
bt_missions.record_img)
img2 = cv2.imread('image/' + str(bt_missions.img_count) + '.PNG',
cv2.IMREAD_GRAYSCALE) #trainImage
# Initiate SIFT detector
sift = cv2.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
# BFMatcher with default params
bf = cv2.BFMatcher()
t = rospy.get_time()
while (rospy.get_time() - t <= 2):
matches = bf.knnMatch(des1, des2, k=2)
# Apply ratio test
good = []
for m, n in matches:
if m.distance < 0.55 * n.distance:
good.append([m])
print("threshold: %s" % len(good))
if len(good) >= 10:
bt_missions.index = bt_missions.index + 1
bt_missions.img_count = 0
bt_missions.kp_count = False
return True
else:
bt_missions.forward_flag = False
return False
@condition
def achieve(self):
if bt_missions.index == 0 and bt_missions.dist_first == True:
bt_missions.dist_first = False
bt_missions.p0 = (bt_missions.home_.geo.latitude,
bt_missions.home_.geo.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print("current dist: %s" % bt_missions.dist)
if bt_missions.dist >= 5:
return False
else:
bt_missions.index = bt_missions.index + 1
elif bt_missions.index == 0 and bt_missions.dist_first == False:
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print("current dist: %s" % bt_missions.dist)
if bt_missions.dist >= 5:
return False
else:
print("achieve first point")
bt_missions.kp_count == True
bt_missions.index = bt_missions.index + 1
return True
elif bt_missions.index == 1 and bt_missions.dist_first == False:
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print("current dist: %s" % bt_missions.dist)
if bt_missions.dist >= 5:
return False
else:
print("achieve key point")
bt_missions.ah_flag = True
bt_missions.kp_count == True
#bt_missions.index = bt_missions.index + 1
return True
elif bt_missions.index == 2 and bt_missions.kp_count == False:
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print("current dist: %s" % bt_missions.dist)
if bt_missions.dist >= 5:
return False
else:
print("achieve final point")
#bt_missions.ah_flag = True
bt_missions.kp_count == True
return True
@condition
def achieve_alt(self):
if bt_missions.current_alt > 15:
print("altitude enough")
return True
else:
print("altitude not enough")
return False
@condition
def yaw_correction(self):
if bt_missions.index == 0:
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.home_.geo.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.home_.geo.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
print("correction heading: %s" % bt_missions.current_yaw)
print("correction brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
return False
else:
return True
else:
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.current_.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.current_.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
print("correction heading: %s" % bt_missions.current_yaw)
print("correction brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
return False
else:
return True
@action
def up(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.linear.z = 0.8
while bt_missions.current_alt <= 15:
#rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.rate.sleep()
print("current altitude : %s" % bt_missions.current_alt)
@action
def correction(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
bt_missions.current_yaw = bt_missions.heading
if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180:
msg.twist.angular.z = 0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng - bt_missions.current_yaw) >= 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng - bt_missions.current_yaw) < 180:
msg.twist.angular.z = 0.1
bt_missions.current_yaw = bt_missions.heading
while bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
#rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.current_yaw = bt_missions.heading
bt_missions.rate.sleep()
print("first correction heading : %s" % bt_missions.current_yaw)
@action
def forward(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.z = 0
print("forward")
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0
vel.velocity.y = 1.0
vel.velocity.z = 0
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
forward_t = rospy.get_time()
while rospy.get_time(
) - forward_t <= bt_missions.dist * 0.1: #bt_missions.dist *1.25*0.25:
#rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.set_v_pub.publish(vel)
bt_missions.rate.sleep()
@action
def change_alt(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.linear.z = 0.8
while bt_missions.current_alt <= 35:
#rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.rate.sleep()
print("current altitude : %s" % bt_missions.current_alt)
def run(self):
while True:
if bt_missions.isContinue == False:
break
bt_count = self.tree.blackboard(1)
bt_state = bt_count.tick()
print("bt state = %s\n" % bt_state)
while bt_count == RUNNING:
bt_state = bt_count.tick()
print("state = %s\n" % bt_state)
assert bt_state == SUCCESS or bt_state == FAILURE
send_init_pub_3 = rospy.Publisher(quad_ros_namespace3 +
'/mavros/home_position/set',
HomePosition,
queue_size=10)
send_init_pub_4 = rospy.Publisher(quad_ros_namespace4 +
'/mavros/home_position/set',
HomePosition,
queue_size=10)
############# THIS IS SOMETHING I TRIED BUT ALSO DID NOT WORK #############
# send_init_pub_1 = rospy.Publisher(quad_ros_namespace1 + '/mavros/setpoint_position/local', PoseStamped, queue_size=10)
# send_init_pub_2 = rospy.Publisher(quad_ros_namespace2 + '/mavros/setpoint_position/local', PoseStamped, queue_size=10)
# send_init_pub_3 = rospy.Publisher(quad_ros_namespace3 + '/mavros/setpoint_position/local', PoseStamped, queue_size=10)
# send_init_pub_4 = rospy.Publisher(quad_ros_namespace4 + '/mavros/setpoint_position/local', PoseStamped, queue_size=10)
# Set up the HomePosition message for each of the quads
p1 = HomePosition()
p1.position.x = 7
p1.position.y = 16
# p1.position.z = 2
# p1.orientation.x = 1
print p1
p2 = HomePosition()
p2.position.x = 9
p2.position.y = 5
# p2.position.z = 2
# p2.orientation.x = 1
print p2
p3 = HomePosition()
p3.position.x = 2
p3.position.y = 9
# p3.position.z = 2
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. """ import rospy import numpy as np import geopy.distance from std_msgs.msg import Int32 from sensor_msgs.msg import NavSatFix from mavros_msgs.msg import State, GlobalPositionTarget, HomePosition from mavros_msgs.srv import CommandHome, CommandBool, SetMode, CommandTOL current_state = State() current_position = NavSatFix() home = HomePosition() wp0 = GlobalPositionTarget() wp1 = GlobalPositionTarget() wp2 = GlobalPositionTarget() wp3 = GlobalPositionTarget() wp4 = GlobalPositionTarget() wp = GlobalPositionTarget() wp_target = GlobalPositionTarget() wp_avoidance = GlobalPositionTarget() wp_helical = GlobalPositionTarget() wp0.coordinate_frame = GlobalPositionTarget().FRAME_GLOBAL_INT wp0.type_mask = GlobalPositionTarget().IGNORE_VX + GlobalPositionTarget( ).IGNORE_VY + GlobalPositionTarget().IGNORE_VZ + GlobalPositionTarget(
def __init__(self, namespace, waypoints, update_frequency=10.):
"""
A Class that interfaces with MAVROS for executing actions
"""
self.namespace = namespace
self.landed = True
self.home_moved = False
self.rel_alt = 0.
self.rangefinder = -1.
self.wp_reached = -1
self.previous_mode = ''
self.current_mode = ''
self._cancel_action = False
self.external_intervened = False
self.state = State()
self.waypoints = list()
self.home = HomePosition()
self.global_pose = NavSatFix()
self.odometry = Odometry()
self.battery_voltages = [self.INIT_VOLTAGE for _ in range(100)]
self.low_battery = False
self._waypoints = waypoints
self._current_wp = -1
self._rel_alt = [0. for _ in range(20)]
self._rangefinder = [-1. for _ in range(30)]
self.rangefinder_msg = Range()
self._min_range = -1.
self._status_text = ''
self._arm_status = [False for _ in range(5)]
# Service proxies
rospy.loginfo('Waiting for service /%s/mavros/cmd/command ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/command' % self.namespace)
self._command_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/command' % self.namespace, CommandLong)
rospy.loginfo('Waiting for service /%s/mavros/mission/push ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/mission/push' % self.namespace)
self._add_wp_proxy = rospy.ServiceProxy(
'/%s/mavros/mission/push' % self.namespace, WaypointPush)
rospy.loginfo(
'Waiting for service /%s/mavros/mission/set_current ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/mission/set_current' %
self.namespace)
self._set_current_wp_proxy = rospy.ServiceProxy(
'/%s/mavros/mission/set_current' % self.namespace,
WaypointSetCurrent)
rospy.loginfo('Waiting for /%s/mavros/set_mode ...' % self.namespace)
rospy.wait_for_service('/%s/mavros/set_mode' % self.namespace)
self._set_mode_proxy = rospy.ServiceProxy(
'/%s/mavros/set_mode' % self.namespace, SetMode)
rospy.loginfo('Wait for service /%s/mavros/mission/clear ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/mission/clear' % self.namespace)
self._clear_wp_proxy = rospy.ServiceProxy(
'/%s/mavros/mission/clear' % self.namespace, WaypointClear)
rospy.loginfo('Waiting for /%s/mavros/cmd/arming ...' % self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/arming' % self.namespace)
self._arming_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/arming' % self.namespace, CommandBool)
rospy.loginfo('Waiting for /%s/mavros/cmd/takeoff ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/takeoff' % self.namespace)
self._takeoff_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/takeoff' % self.namespace, CommandTOL)
rospy.loginfo('Waiting for /%s/mavros/cmd/set_home ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/set_home' % self.namespace)
self._set_home_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/set_home' % self.namespace, CommandHome)
self._rate = rospy.Rate(update_frequency)
# Publisher
self._setpoint_pub = rospy.Publisher(
'/%s/mavros/setpoint_position/global' % self.namespace,
GeoPoseStamped,
queue_size=10)
# Subscribers
rospy.Subscriber('/%s/mavros/state' % self.namespace,
State,
self._state_cb,
queue_size=10)
# halt until mavros is connected to a uav
rospy.loginfo('Waiting for a connection between MAVROS and UAV ...')
while (not self.state.connected):
self._rate.sleep()
rospy.Subscriber('/%s/mavros/home_position/home' % self.namespace,
HomePosition,
self._home_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/global_position/rel_alt' % self.namespace,
Float64,
self._relative_alt_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/battery' % self.namespace,
BatteryState,
self._battery_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/global_position/global' % self.namespace,
NavSatFix,
self._global_pose_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/mission/reached' % self.namespace,
WaypointReached,
self._wp_reached_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/statustext/recv' % self.namespace,
StatusText,
self._status_text_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/distance_sensor/rangefinder_sub' %
self.namespace,
Range,
self._rangefinder_cb,
queue_size=10)
rospy.Subscriber('/%s/mavros/global_position/local' % self.namespace,
Odometry,
self._odometry_cb,
queue_size=10)
# Auto call functions
rospy.Timer(self._rate.sleep_dur, self.update_wp_position)
rospy.Timer(self._rate.sleep_dur, self.update_landing_status)
rospy.Timer(10 * self._rate.sleep_dur, self.intervene_observer)
rospy.loginfo('Adding WPs ...')
# Adding initial waypoints' configuration
while not self.add_waypoints():
self._rate.sleep()
from mavros import setpoint as SP
from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import PoseStamped, TwistStamped, Twist, Vector3, Pose
from mavros_msgs.srv import CommandBool, SetMode, CommandTOL
from mavros_msgs.msg import HomePosition, GlobalPositionTarget, PositionTarget, Altitude
from sensor_msgs.msg import NavSatFix
from math import sin, cos, atan2, degrees, acos, radians
from geopy.distance import vincenty
from mavros_msgs.msg import State
from std_msgs.msg import Float64
rospy.init_node('mavros_takeoff_python')
mavros.set_namespace()
rate = rospy.Rate(10)
dirct_ = Float64()
home_ = HomePosition()
current_ = NavSatFix()
gps_state_ = NavSatFix()
home_set_ = False
global lat1, long1, lat2, long2
lat1 = 24.984105
long1 = 121.572514
lat2 = 24.98404
long2 = 121.572775
lat3 = 24.984139
long3 = 121.57305
global dist
global lat, lon
global brng
lat = 0
lon = 0
import rospy
from mavros_msgs.msg import HomePosition
from geometry_msgs.msg import Point, PoseStamped, Quaternion
viconDat = None
def viconCB(msg):
global viconDat
viconDat = msg
rospy.init_node('set_home_node', anonymous=True)
rospy.loginfo("Publishing: '/mavros/home_position/set'")
rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, viconCB)
# Publish home position
pubHome = rospy.Publisher('/mavros/home_position/set',
HomePosition,
queue_size=10)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
#if viconDat is not None:
if viconDat is None:
msg = HomePosition()
#msg.position = #viconDat.pose.position
#msg.orientation = #viconDat.pose.orientation
msg.header.stamp = rospy.Time.now()
pubHome.publish(msg)
def __init__(self, namespace, waypoints, update_frequency=10.):
"""
A Class that interfaces with MAVROS for executing actions
"""
self.current_mode = ''
self.previous_mode = ''
self.namespace = namespace['name']
self.fuel_rate_mean = 1.0
self.fuel_rate_std = 1.0
self.low_fuel = False
self.fuel = self.INIT_FUEL
self.set_battery(namespace['max_fuel'], namespace['min_fuel'],
namespace['fuel_rate'])
self._cancel_action = False
self.external_intervened = False
self.state = State()
self.waypoints = waypoints
self.home = HomePosition()
self.global_pose = NavSatFix()
self.heading = 0.0
self._current_wp = -1
self._radius = 1e-04
self._rate = rospy.Rate(update_frequency)
# Service proxies
rospy.loginfo('Waiting for /%s/mavros/set_mode ...' % self.namespace)
rospy.wait_for_service('/%s/mavros/set_mode' % self.namespace)
self._set_mode_proxy = rospy.ServiceProxy(
'/%s/mavros/set_mode' % self.namespace, SetMode)
rospy.loginfo('Waiting for /%s/mavros/cmd/arming ...' % self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/arming' % self.namespace)
self._arming_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/arming' % self.namespace, CommandBool)
rospy.loginfo('Waiting for /%s/mavros/cmd/set_home ...' %
self.namespace)
rospy.wait_for_service('/%s/mavros/cmd/set_home' % self.namespace)
self._set_home_proxy = rospy.ServiceProxy(
'/%s/mavros/cmd/set_home' % self.namespace, CommandHome)
# UAV service proxies for updating UAV home position
if len(namespace['uav_onboard']):
self._uav_home_proxies = {
uav: rospy.ServiceProxy('/%s/mavros/cmd/set_home' % uav,
CommandHome)
for uav in namespace['uav_onboard']
}
self.uav_home_wp = {
uav: HomePosition()
for uav in namespace['uav_onboard']
}
self._uav_home_offset = {
uav: np.ones(4) * float('inf')
for uav in namespace['uav_onboard']
}
self._uav_home_pose_pub = {
uav: rospy.Publisher(
'/%s_launchpad/mavros/global_position/raw/unfix' % uav,
NavSatFix,
queue_size=3)
for uav in namespace['uav_onboard']
}
self._uav_home_heading_pub = {
uav: rospy.Publisher(
'/%s_launchpad/mavros/global_position/compass_hdg' % uav,
Float64,
queue_size=3)
for uav in namespace['uav_onboard']
}
for uav in namespace['uav_onboard']:
rospy.Subscriber('/%s/mavros/home_position/home' % uav,
HomePosition,
lambda i: self._uav_home_cb(i, uav),
queue_size=1)
rospy.Timer(2 * self._rate.sleep_dur, self.update_uav_home_pos)
# Subscribers
rospy.Subscriber('/%s/mavros/state' % self.namespace,
State,
self._state_cb,
queue_size=1)
# halt until mavros is connected to a asv
rospy.loginfo('Waiting for a connection to %s ...' % self.namespace)
while (not self.state.connected):
self._rate.sleep()
rospy.Subscriber('/%s/mavros/global_position/compass_hdg' %
self.namespace,
Float64,
self._heading_cb,
queue_size=1)
rospy.Subscriber('/%s/mavros/home_position/home' % self.namespace,
HomePosition,
self._home_cb,
queue_size=1)
rospy.Subscriber('/%s/mavros/modified_battery' % self.namespace,
BatteryState,
self._battery_cb,
queue_size=1)
rospy.Subscriber('/%s/mavros/global_position/raw/unfix' %
self.namespace,
NavSatFix,
self._global_pose_cb,
queue_size=1)
# Publisher
self._setpoint_pub = rospy.Publisher('/%s/mavros/setpoint_raw/global' %
self.namespace,
GlobalPositionTarget,
queue_size=3)
self._rotate_cam = rospy.Publisher('/%s/activate_rotation' %
self.namespace,
Int32,
queue_size=3)
# Auto call functions
rospy.Timer(10 * self._rate.sleep_dur, self.intervene_observer)
rospy.Timer(self._rate.sleep_dur, self.update_wp_position)
# change mode just to fill self.current_mode and self.previous_mode
self.guided_mode()
# Adding initial waypoints' configuration
self.set_current_location_as_home()
def setUp(self):
print('@ctrl_server@ uav{0} setUp'.format(self.uav_number))
self.pos = PoseStamped()
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.misson_wp = WaypointList()
self.state = State()
self.scan = LaserScan()
self.mav_type = None
self.ready = False
# Target offset radius
self.radius = 0.25
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state', 'scan'
]
}
# ROS services
service_timeout = 60
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service(self.uav_prefix + 'mavros/param/get', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/cmd/arming', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/mission/push', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/mission/clear', service_timeout)
rospy.wait_for_service(self.uav_prefix + 'mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
print("@ctrl_server@ failed to connect to services")
self.get_param_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/param/get',
ParamGet)
self.set_arming_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/set_mode',
SetMode)
self.wp_clear_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber(self.uav_prefix + 'mavros/altitude',
Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber(self.uav_prefix + 'mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber(self.uav_prefix + 'mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber(self.uav_prefix + 'mavros/state', State,
self.state_callback)
self.get_scan_srv = rospy.Subscriber('iris_rplidar_' + self.uav_number + '/laser/scan',
LaserScan,
self.scan_callback)
# ROS publisers
self.pos_setpoint_pub = rospy.Publisher(
self.uav_prefix + 'mavros/setpoint_position/local', PoseStamped, queue_size=1)
# send setpoints in seperate thread
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
print('@ctrl_server@ setUp over')
pass
def __init__(self, update_frequency=10.):
"""
A Class that interfaces ROSPlan and MAVROS for executing actions
"""
self.landed = True
self.home_moved = False
self.rel_alt = 0.
self.wp_reached = -1
self.previous_mode = ''
self.current_mode = ''
self.external_intervened = False
self.state = State()
self.waypoints = list()
self.home = HomePosition()
self.compass_hdg = Float64()
self.global_pose = NavSatFix()
self.battery_voltages = [self.INIT_VOLTAGE for _ in range(30)]
self.low_battery = False
self._current_wp = -1
self._rel_alt = [0. for _ in range(15)]
self._rel_alt_seq = 0
# Service proxies
rospy.loginfo('Waiting for service /mavros/cmd/command ...')
rospy.wait_for_service('/mavros/cmd/command')
self._command_proxy = rospy.ServiceProxy('/mavros/cmd/command',
CommandLong)
rospy.loginfo('Waiting for service /mavros/mission/push ...')
rospy.wait_for_service('/mavros/mission/push')
self._add_wp_proxy = rospy.ServiceProxy('/mavros/mission/push',
WaypointPush)
rospy.loginfo('Waiting for service /mavros/mission/set_current ...')
rospy.wait_for_service('/mavros/mission/set_current')
self._set_current_wp_proxy = rospy.ServiceProxy(
'/mavros/mission/set_current', WaypointSetCurrent)
rospy.loginfo('Waiting for /mavros/set_mode ...')
rospy.wait_for_service('/mavros/set_mode')
self._set_mode_proxy = rospy.ServiceProxy('/mavros/set_mode', SetMode)
rospy.loginfo('Wait for service /mavros/mission/clear ...')
rospy.wait_for_service('/mavros/mission/clear')
self._clear_wp_proxy = rospy.ServiceProxy('/mavros/mission/clear',
WaypointClear)
rospy.loginfo('Waiting for /mavros/cmd/arming ...')
rospy.wait_for_service('/mavros/cmd/arming')
self._arming_proxy = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
rospy.loginfo('Waiting for /mavros/cmd/takeoff ...')
rospy.wait_for_service('/mavros/cmd/takeoff')
self._takeoff_proxy = rospy.ServiceProxy('/mavros/cmd/takeoff',
CommandTOL)
rospy.loginfo('Waiting for /mavros/cmd/set_home ...')
rospy.wait_for_service('/mavros/cmd/set_home')
self._set_home_proxy = rospy.ServiceProxy('/mavros/cmd/set_home',
CommandHome)
self._rate = rospy.Rate(update_frequency)
# Subscribers
rospy.Subscriber('/mavros/state', State, self._state_cb, queue_size=10)
# halt until mavros is connected to a uav
rospy.loginfo('Waiting for a connection between MAVROS and UAV ...')
while (not self.state.connected):
self._rate.sleep()
rospy.Subscriber('/mavros/home_position/home',
HomePosition,
self._home_cb,
queue_size=10)
rospy.Subscriber('/mavros/global_position/rel_alt',
Float64,
self._relative_alt_cb,
queue_size=10)
rospy.Subscriber('/mavros/battery',
BatteryState,
self._battery_cb,
queue_size=10)
rospy.Subscriber('/mavros/global_position/global',
NavSatFix,
self._global_pose_cb,
queue_size=10)
rospy.Subscriber('/mavros/global_position/compass_hdg',
Float64,
self._compass_hdg_cb,
queue_size=10)
rospy.Subscriber('/mavros/mission/reached',
WaypointReached,
self._wp_reached_cb,
queue_size=10)
# Auto call functions
rospy.Timer(rospy.Duration(1. / update_frequency),
self.update_landing_status)
rospy.Timer(rospy.Duration(20. / update_frequency),
self.overwatch_current_mode)
rospy.Timer(rospy.Duration(1. / update_frequency),
self.update_wp_position)
rospy.loginfo('Adding WPs ...')
self._wait(10)
# Adding initial waypoints' configuration
while not self.add_waypoints():
self._rate.sleep()
class bt_missions:
rospy.init_node('mavros_takeoff_python')
mavros.set_namespace()
rate = rospy.Rate(20)
dirct_ = Float64()
home_ = HomePosition()
current_state = State()
current_ = GlobalPositionTarget()
home_set_ = False
isContinue = True
dist_first = True
dis_count = True
#triangle
target = [[24.985059, 121.572934], [24.985161, 121.572858]]
forward_t = rospy.get_time()
index = 0
time_index = 0
p0 = 0
p1 = 0
brng = 0
dLon = 0
y = 0
x = 0
current_yaw = 0
dist = 0
heading = 0
lat = 0
lon = 0
cnt = 0
set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
set_v_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position',
'local'),
PoseStamped,
queue_size=10)
def __init__(self):
self.hdg_subscriber = rospy.Subscriber(
"/mavros/global_position/compass_hdg", Float64, self.compass)
self.home_sub = rospy.Subscriber("/mavros/home_position/home",
HomePosition, self.setHomeGeoPointCB)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global",
NavSatFix, self.current_position_cb)
state_sub = rospy.Subscriber(mavros.get_topic('state'), State,
self.state_cb)
self.tree = self.acheive | (
(self.isGuided | (self.notGuided >> self.set_guided)) >>
(self.yaw_correction | self.notGuided | self.notGuided
| self.correction) >> self.forward)
#(self.yaw_discorrection | self.notGuided | (self.yaw_correction >>
def compass(self, dirct):
bt_missions.dirct_ = dirct
bt_missions.heading = bt_missions.dirct_.data
def state_cb(self, state):
bt_missions.current_state = State()
bt_missions.current_state = state
def angleFromCoordinate(self, lat0, long0, lat1, long1):
bt_missions.dLon = (long1 - long0)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(bt_missions.lat1)
bt_missions.x = math.cos(lat0) * math.sin(lat1) - math.sin(lat0) * cos(
lat1) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
def setHomeGeoPointCB(self, home):
bt_missions.home_ = home
bt_missions.home_set_ = True
rospy.loginfo(
"Received Home (WGS84 datum): %lf, %lf, %lf" %
(bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude,
bt_missions.home_.geo.altitude))
def current_position_cb(self, current):
bt_missions.current_ = current
bt_missions.lat = bt_missions.current_.latitude
bt_missions.lon = bt_missions.current_.longitude
@condition
def acheive(self):
if bt_missions.index == 0 and bt_missions.dist_first == True:
bt_missions.dist_first = False
bt_missions.p0 = (bt_missions.home_.geo.latitude,
bt_missions.home_.geo.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print(bt_missions.dist)
if bt_missions.dist >= 1.5:
return False
else:
bt_missions.index = bt_missions.index + 1
elif bt_missions.index == 0 and bt_missions.dist_first == False:
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print(bt_missions.dist)
if bt_missions.dist >= 1.5:
return False
else:
bt_missions.index = bt_missions.index + 1
elif bt_missions.index == 1:
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print(bt_missions.dist)
if bt_missions.dist >= 1.5:
return False
else:
return True
@condition
def isGuided(self):
if bt_missions.current_state == "GUIDED":
return True
else:
return False
@condition
def notGuided(self):
if bt_missions.current_state == "GUIDED":
return False
else:
return True
@condition
def yaw_correction(self):
if bt_missions.index == 0:
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.home_.geo.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.home_.geo.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
return False
else:
return True
else:
bt_missions.dis_count = True
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.current_.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.current_.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
return False
else:
return True
@condition
def yaw_discorrection(self):
if bt_missions.index == 0:
bt_missions.current_yaw = bt_missions.heading
print("discorrection heading: %s" % bt_missions.current_yaw)
print("discorrection brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
bt_missions.current_yaw = bt_missions.heading
return True
else:
return False
else:
bt_missions.current_yaw = bt_missions.heading
print("discorrection heading: %s" % bt_missions.current_yaw)
print("discorrection brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
bt_missions.current_yaw = bt_missions.heading
return True
else:
return False
@action
def set_guided(self):
rospy.wait_for_service('/mavros/set_mode')
try:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
except rospy.ServiceException as e:
print("Set mode failed: %s" % e)
@action
def correction(self):
if bt_missions.dis_count == True:
bt_missions.current_yaw = bt_missions.heading
while bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
msg = TwistStamped()
if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180:
msg.twist.angular.z = 0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng -
bt_missions.current_yaw) >= 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng -
bt_missions.current_yaw) < 180:
msg.twist.angular.z = 0.1
bt_missions.set_vel_pub.publish(msg)
bt_missions.current_yaw = bt_missions.heading
#print("first correction : %s" %bt_missions.current_yaw)
else:
#pass
bt_missions.current_yaw = bt_missions.heading
while bt_missions.current_yaw > (
360 - bt_missions.brng
) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng
) - 2: #and int(rospy.get_time() -bt_missions.forward_t) % 10 == 0:
msg = TwistStamped()
#print("forward correction: %s" %bt_missions.current_yaw)
msg.twist.angular.z = math.radians(bt_missions.current_yaw -
360 + bt_missions.brng)
bt_missions.set_vel_pub.publish(msg)
bt_missions.current_yaw = bt_missions.heading
@action
def forward(self):
print(123)
forward_t = rospy.get_time()
#while rospy.get_time() - forward_t <= 5:
bt_missions.dis_count = False
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0
vel.velocity.y = 0.8
vel.velocity.z = 0
bt_missions.set_v_pub.publish(vel)
bt_missions.rate.sleep()
def run(self):
while True:
if bt_missions.isContinue == False:
break
bt_count = self.tree.blackboard(1)
bt_state = bt_count.tick()
print("bt state = %s\n" % bt_state)
while bt_count == RUNNING:
bt_state = bt_count.tick()
print("state = %s\n" % bt_state)
assert bt_state == SUCCESS or bt_state == FAILURE
