def heartbeat(hsock):
request = messages_pb2.Request()
request.command = messages_pb2.Command.HEART_BEAT
data = request.SerializeToString()
print(len(data))
hsock.sendto(data, ('127.0.0.1', 4444))
received = hsock._recv_msg(1024)
reply = messages_pb2.Reply()
reply.ParseFromString(received)
a = BytesIO()
a.write(reply.heartbeatData.mavrosState)
mavros_state = State()
mavros_state.deserialize(reply.heartbeatData.mavrosState)
print (reply)
print (mavros_state)
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 __init__(self):
self.state = "disarm"
#создаем топики, для публикации управляющих значений:
self.pub_pt = rospy.Publisher(f"/mavros{1}/setpoint_raw/local",
PositionTarget,
queue_size=10)
self.pt = PositionTarget()
self.pt.coordinate_frame = self.pt.FRAME_LOCAL_NED
self.t0 = time.time()
self.dt = 0
#params
self.p_gain = 2
self.max_velocity = 5
self.arrival_radius = 0.6
self.waypoint_list = [
np.array([6., 7., 6.]),
np.array([0., 14., 7.]),
np.array([18., 14., 7.]),
np.array([0., 0., 5.])
]
self.current_waypoint = np.array([0., 0., 0.])
self.pose = np.array([0., 0., 0.])
self.velocity = np.array([0., 0., 0.])
self.mavros_state = State()
self.subscribe_on_topics()
def setup_mavros(self):
self.mavros_state = State()
self.mavros_state.connected = False
self.arming_client = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.landing_client = rospy.ServiceProxy('mavros/cmd/land', CommandTOL)
self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
self.pos_sub = rospy.Subscriber('/mavros/local_position/pose',
PoseStamped, self.position_cb)
self.vel_sub = rospy.Subscriber(
'/mavros/local_position/velocity_local', TwistStamped,
self.velocity_cb)
self.global_pos_sub = rospy.Subscriber(
'/mavros/global_position/global', NavSatFix,
self.global_position_cb)
self.state.pose.pose.orientation.x = 0.0
self.state.pose.pose.orientation.y = 0.0
self.state.pose.pose.orientation.z = 0.0
self.state.pose.pose.orientation.w = 1.0
def __init__(self):
# Communication variables
self._last_pose = PoseStamped()
self._setpoint_msg = PositionTarget()
self._setpoint_msg.type_mask = _DEFAULT
self._last_state = State()
# ROS Communication
self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local",
PositionTarget, queue_size=1)
self._pose_sub = rospy.Subscriber("mavros/local_position/pose",
PoseStamped, self._pose_cb)
self._state_sub = rospy.Subscriber('mavros/state',
State, self._state_cb)
# wait for mavros to start publishing
rospy.logdebug("Waiting for MAVROS to start")
rospy.wait_for_message("mavros/mission/waypoints", WaypointList)
# Make drone less aggressive
rospy.wait_for_service("mavros/param/set")
mavparam = rospy.ServiceProxy('mavros/param/set', ParamSet)
mavparam("MC_PITCH_P", ParamValue(0, 2.0)).success
mavparam("MC_ROLL_P", ParamValue(0, 2.0)).success
mavparam("MPC_XY_VEL_MAX", ParamValue(0, 3.0)).success
# Send a few setpoint to make MAVROS happy
rate = rospy.Rate(20.0)
for _ in range(40):
rate.sleep()
self._publish_setpoint(None)
rospy.Timer(rospy.Duration(0.05), self._publish_setpoint)
rospy.loginfo("Drone Initialized")
def __init__(self, control_reference_frame='bu'):
# Create node with name 'controller'
rospy.init_node('translation_controller')
if control_reference_frame not in _COORDINATE_FRAMES:
raise ValueError("Invalid control reference frame: " + control_reference_frame)
self.control_reference_frame=control_reference_frame
# A subscriber to the topic '/mavros/local_position/pose. self.pos_sub_cb is called when a message of type 'PoseStamped' is recieved
self.pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pos_sub_cb)
# Quaternion representing the rotation of the drone's body frame (bu) in the LENU frame.
# Initiallize to identity quaternion, as bu is aligned with lenu when the drone starts up.
self.quat_bu_lenu = (0, 0, 0, 1)
# A subscriber to the topic '/mavros/state'. self.state_sub_cb is called when a message of type 'State' is recieved
self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_sub_cb)
# Flight mode of the drone ('OFFBOARD', 'POSCTL', 'MANUAL', etc.)
self.mode = State().mode
# A publisher which will publish the desired linear and anglar velocity to the topic '/setpoint_velocity/cmd_vel_unstamped'
self.velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size = 1)
# Initialize linear setpoint velocities
self.vx = 0
self.vy = 0
self.vz = 0
# Publishing rate
self.rate = rospy.Rate(_RATE)
# Boolean used to indicate if the streaming thread should be stopped
self.stopped = False
def __init__(self):
#rospy.init_node('align')
self.drone_state = State()
#rospy.init_node('align_reference', anonymous=True)
self.rate = rospy.Rate(20) # 20hz
rospy.Subscriber("/cv_detection/color_range/detection", Point, self.point_callback, queue_size=None)
# rospy.Subscriber(
# "/control/align_reference/set_image_shape", Point, self.set_image_shape, queue_size=None)
# rospy.Subscriber(
# "/control/align_reference/set_goal_point", Point, self.set_goal_point, queue_size=None)
# rospy.Subscriber(
# "/control/align_reference/set_camera_angle", Float, self.set_camera_angle, queue_size=None)
self.setpoint_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=1)
self.alginned = rospy.Publisher(
"/control/align_reference/aligned", Bool, queue_size=1)
'''
Usar, condicionalmente /mavros/state e drone_state
Parametrizar estado /mavros/state com rosparam
'''
rospy.Subscriber("/mavros/state", State, self.state_callback, queue_size=1)
self.enable = ""
rospy.Subscriber("pack/node/set", String, self.enable_cb, queue_size=None)
self.return_pub = rospy.Publisher("pack/node/return", String, queue_size=1)
def __init__(self):
# Communication variables
self._last_pose = None
self._last_twist = None
self._setpoint_msg = PositionTarget()
self._last_state = State()
self._camera = Camera(imgtopic="drone/camera_front/image",
infotopic="drone/camera_front/info")
# ROS Communication
self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local",
PositionTarget,
queue_size=1)
self._pose_sub = rospy.Subscriber("mavros/local_position/pose",
PoseStamped, self._pose_cb)
self._twist_sub = rospy.Subscriber("mavros/local_position/velocity",
TwistStamped, self._twist_cb)
self._state_sub = rospy.Subscriber('mavros/state', State,
self._state_cb)
self._publish_setpoint_active = False
rospy.Timer(rospy.Duration(0.05), self._publish_setpoint)
rospy.loginfo("Drone Initialized")
def __init__(self):
rospy.init_node("mav_control_node")
rospy.Subscriber("/mavros/global_position/global",NavSatFix,self.gps_callback)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback)
rospy.Subscriber('mavros/state',State,self.arm_callback)
rospy.Subscriber('mavros/battery',BatteryState,self.battery_callback)
rospy.Subscriber('mavros/global_position/raw/satellites',UInt32,self.sat_num_callback)
rospy.Subscriber('mavros/global_position/rel_alt',Float64,self.rel_alt_callback)
rospy.Subscriber('mavros/imu/data',Imu,self.imu_callback)
rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback)
rospy.Subscriber('mavros/vfr_hud',VFR_HUD,self.hud_callback)
self.gps = NavSatFix()
self.pose = PoseStamped()
self.arm_status = State()
self.battery_status = BatteryState()
self.sat_num = UInt32()
self.rel_alt = Float64()
self.imu = Imu()
self.rc = RCIn()
self.hud = VFR_HUD()
self.timestamp = rospy.Time()
self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1)
self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1)
self.mode_service = rospy.ServiceProxy("/mavros/set_mode", SetMode)
self.arm_service = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
self.takeoff_service = rospy.ServiceProxy("/mavros/cmd/takeoff", CommandTOL)
self.stream_service = rospy.ServiceProxy("/mavros/set_stream_rate", StreamRate)
self.home_service = rospy.ServiceProxy("/mavros/cmd/set_home", CommandHome)
def __init__(self):
#initializing the class variables
self.pose = PoseStamped()
self.glob_pos = NavSatFix()
self.velocity = TwistStamped()
self.state = State()
#self.bridge = CvBridge()
# streaming of subscribers
rospy.wait_for_service("mavros/set_stream_rate")
set_stream_rate = rospy.ServiceProxy("mavros/set_stream_rate",
StreamRate)
set_stream_rate(StreamRateRequest.STREAM_POSITION, 50, True)
set_stream_rate(StreamRateRequest.STREAM_EXTRA1, 50, True)
#camera
#self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback)
# position
rospy.Subscriber("/mavros/local_position/pose", PoseStamped,
self.process_position)
# global position
rospy.Subscriber("/mavros/global_position/global", NavSatFix,
self.glob_posi)
#mode
rospy.Subscriber("/mavros/state", State, self.process_state)
# velocity
rospy.Subscriber("/mavros/local_position/velocity_local", TwistStamped,
self.process_velocity)
def __init__(self, controller, a_id, namespace, address, rx_port):
self.calibration_path = Path()
self.controller = controller
self.a_id = a_id
self.namespace = namespace
self.local_map_name = '%s_map' % (namespace, )
self.local_map_transform = TransformStamped()
self.local_map_transform_calculated = False
self.heartbeat_time = None
self.command_id = 1
self.mavros_state = MavrosState()
self.local_pose = PoseStamped()
self.global_position = NavSatFix()
self.rx_port = rx_port
self._create_socket(address)
self.recv_q = Queue.Queue()
self.recv_thread = threading.Thread(target=self._recv_thread)
self.recv_thread.start()
self.publishers = {}
self.subscribers = {}
self._register_publisher()
self._register_subscribers()
self._create_calibration_path()
self.calibration_poses = []
self.command_thread = None
self.last_command = (1, True
) # last_command_id, last_command_completed
rospy.loginfo("%s: Initialized", self.namespace)
def network_state_changed_callback(self, new_num_nodes):
self.num_nodes_in_swarm = new_num_nodes
for i in range(1, self.num_nodes_in_swarm.data + 1):
uav_name = "uav" + str(i)
if (i != self.id
): # Create objects to use to subscribe to neighbor stats
if i not in self.neighbor_position_objects.keys():
self.neighbor_position_objects[str(i)] = PoseStamped()
self.neighbor_global_position_objects[str(i)] = NavSatFix()
self.neighbor_velocity_objects[str(i)] = TwistStamped()
self.neighbor_state_objects[str(i)] = State()
if i not in self.neighbor_position_objects.keys():
self.neighbor_state_sub[str(i)] = rospy.Subscriber(
uav_name + "/mavros/state", State,
self.neighbor_state_changed_callback, i)
self.neighbor_pos_sub[str(i)] = rospy.Subscriber(
uav_name + "/mavros/local_position/pose", PoseStamped,
self.neighbor_position_changed_callback, i)
self.neighbor_vel_sub[str(i)] = rospy.Subscriber(
uav_name + "/mavros/local_position/velocity_local",
TwistStamped, self.neighbor_velocity_changed_callback,
i)
self.neighbor_global_pos_sub[str(i)] = rospy.Subscriber(
uav_name + "/mavros/global_position/global", NavSatFix,
self.neighbor_global_pos_changed_callback, i)
def __init__(self):
self.pose = PoseStamped()
self.radius = 0.1
self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=1)
self.state_sub = rospy.Subscriber('mavros/state', State, self.state_cb)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
except rospy.ROSInterruptException:
self.fail("failed to connect to service")
self.arming_client = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
# callback method for state sub
self.current_state = State()
self.offb_set_mode = SetMode
#new threading for self.send_pos. and daemon with true means the threading while stoped with the main one.
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
def setUp(self):
self.local_position = PoseStamped()
self.state = State()
self.pos = PoseStamped()
self.sub_topics_ready = {
key: False
for key in ['local_pos', 'home_pos', 'state']
}
# setup ROS topics and services
try:
rospy.wait_for_service('mavros/cmd/arming', 30)
rospy.wait_for_service('mavros/set_mode', 30)
except rospy.ROSException:
self.fail("failed to connect to mavros services")
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=10)
# 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, drone_id):
# self.id = 0
self.id = drone_id
self.id_state = 0
self.id_pub = rospy.Publisher("ID", Int32)
self.arming_cmd = CommandBool()
self.arming_cmd.value = True
rospy.init_node("controller", anonymous=True)
rospy.Subscriber("request_ID", Int32, self.get_id, queue_size=1)
time.sleep(1)
while self.id_state == 0:
self.id_pub.publish(self.id_state)
print("Waiting for ID")
time.sleep(0.5)
self.rate = rospy.Rate(20)
print("ID received", self.id)
rospy.Subscriber(str(self.id) + "/mavros/state", State, self.state_cb)
rospy.Subscriber(
str(self.id) + "/mavros/local_position/pose", PoseStamped,
self.update_pose)
self.set_mode_client = rospy.ServiceProxy(
str(self.id) + "/mavros/set_mode", SetMode)
self.local_pos_pub = rospy.Publisher(str(self.id) +
"/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
self.state = State()
self.pose = PoseStamped()
self.mode = self.state.mode
self.target = PoseStamped()
time.sleep(2)
def init(mission):
global state, setpoint, run, pose, home, init
global arming_client, set_mode_client
#Global variable initialisation
state, pose, setpoint, home = State(), PoseStamped(), Point(), Point()
run = True
init = True
# Node initiation
rospy.init_node(NODE_NAME)
rospy.loginfo('node initializeed')
time.sleep(1)
# setting up the pins for electromagnet
wpi.wiringPiSetup()
wpi.pinMode(0,1)
# Publishers, subscribers and services
pose_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, Pose_Callback)
state_sub = rospy.Subscriber('/mavros/state', State, State_Callback)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
# Thread to send setpoints - mission execution
tSetPoints = Thread(target=sendSetpoint,args=(mission,)).start()
# Monitor security
while not rospy.is_shutdown():
InterfaceKeyboard()
def __init__(self):
# rospy.loginfo("ARDistChecker Started!")
self.ar_pose_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers,
self.ar_pose_cb)
# A subscriber to the topic '/mavros/state'. self.state is called when a message of type 'State' is recieved
self.state_subscriber = rospy.Subscriber("/mavros/state", State,
self.update_state)
# State of the drone. self.state.mode = flight mode, self.state.armed = are motors armed (True or False), etc.
self.state = State()
# List of all the "captured" markers
self.captured = []
# The closest marker to the drone (in the z direction)
self.marker = None
# Points to the next marker in Constants.MARKERS that needs to be captured
self.index = 0
if len(MARKERS) > 0:
# True if all markers have been captured, Fasle otherwise
self.done = False
else:
rospy.loginfo("DONE!")
self.done = True
def __init__(self, mav_name, mav_type="mavros"):
#rospy.init_node("MAV_" + mav_name)
self.rate = rospy.Rate(20)
self.drone_pose = PoseStamped()
self.goal_pose = PoseStamped()
self.goal_vel = TwistStamped()
self.drone_state = State()
self.battery = BatteryState()
############## Publishers ###############
self.local_position_pub = rospy.Publisher(
CONFIG[mav_type + "_local_position_pub"],
PoseStamped,
queue_size=20)
self.velocity_pub = rospy.Publisher(CONFIG[mav_type + "_velocity_pub"],
TwistStamped,
queue_size=5)
self.mavlink_pub = rospy.Publisher('/mavlink/to',
Mavlink,
queue_size=1)
########## Subscribers ##################
self.local_atual = rospy.Subscriber(CONFIG[mav_type + "_local_atual"],
PoseStamped, self.local_callback)
self.state_sub = rospy.Subscriber(CONFIG[mav_type + "_state_sub"],
State, self.state_callback)
self.battery_sub = rospy.Subscriber("/mavros/battery", BatteryState,
self.battery_callback)
############# Services ##################
self.arm = rospy.ServiceProxy(CONFIG[mav_type + "_arm"],
mavros_msgs.srv.CommandBool)
self.set_mode = rospy.ServiceProxy(CONFIG[mav_type + "_set_mode"],
mavros_msgs.srv.SetMode)
def __init__(self):
self.local_pose_subscriber_prev = 0
startup_start = timeit.default_timer()
print('start')
# Rate init
self.rate = rospy.Rate(20.0) # MUST be more then 2Hz
self.current_state = State()
# Current state sub
self.state_sub = rospy.Subscriber("mavros/state", State,
self.state_subscriber_callback)
# Init last_request
self.last_request = rospy.get_rostime()
# wait for FCU connection
print('Waiting for FCU connection...')
while not self.current_state.connected:
self.rate.sleep()
print('Connection sucessful!!')
startup_stop = timeit.default_timer()
print('startup time is' + str(startup_stop - startup_start))
# Quadcopter imu subscriber init
attitude_target_sub = rospy.Subscriber(
"/mavros/setpoint_raw/target_attitude", AttitudeTarget,
self.attitude_target_sub_callback)
def __init__(self, rate=20):
# Rate has to be greater than 2Hz for px4
self.rate = rospy.Rate(rate)
self.current_state = State()
self.pose = PoseStamped()
self.setpoint_pos = PoseStamped()
self.vel = TwistStamped()
self.setpoint_vel = Twist()
self.state_sub = rospy.Subscriber("mavros/state",
State,
callback=self.state_cb)
self.pose_sub = rospy.Subscriber("mavros/local_position/pose",
PoseStamped,
callback=self.pose_cb)
self.vel_sub = rospy.Subscriber("mavros/local_position/velocity",
TwistStamped,
callback=self.vel_cb)
self.setpoint_pos_pub = rospy.Publisher(
"mavros/setpoint_position/local", PoseStamped, queue_size=10)
self.setpoint_vel_pub = rospy.Publisher(
"mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=10)
rospy.wait_for_service('mavros/cmd/arming')
self.arm_service = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
rospy.wait_for_service('mavros/set_mode')
self.set_mode_service = rospy.ServiceProxy('mavros/set_mode', SetMode)
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():
# Input data
# Output data
global state, setpoint, yawSetPoint, setPointsCount, PositionsCount, \
run, laser_position_count, laserposition, pose, lasers_raw, position_control
# Publishers
global local_pos_pub, arming_client, set_mode_client
# Objects
# Global variable initialisation
lasers_raw = Distance(lasers=[0, 0, 0, 0, 0, 0], status=[0, 0, 0, 0, 0, 0])
pose = PoseStamped()
laserposition = PoseStamped()
setpoint = Point()
setpoint.x = 1
setpoint.y = 1
setpoint.z = 1
# When true, setpoints are positions
# When false, setpoints is a velocity
position_control = True
yawSetPoint = 0
laser_position_count = 0
run = True
setPointsCount = 0
PositionsCount = 0
state = State()
# Node initiation
rospy.init_node('laserpack_control')
local_pos_pub = rospy.Publisher('mavros/mocap/pose',
PoseStamped,
queue_size=1)
time.sleep(1)
# Publishers, subscribers and services
pose_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped,
Pose_Callback)
laser_pose_sub = rospy.Subscriber('lasers/filtered', PoseStamped,
laser_callback)
state_sub = rospy.Subscriber('mavrqos/state', State, State_Callback)
laser_sub = rospy.Subscriber('lasers/raw', Distance, lasers_raw_callback)
task_sub = rospy.Subscriber('web/task', Task, Task_Callback)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
# Thread to send setpoints
tSetPoints = Thread(target=sendSetpoint).start()
# Thread to send Lidar height
tLidarZ = Thread(target=sendLidar).start()
# In case we want to send positions with reduced rate, just use this thread
# tPositions = Thread(target=sendPosition).start()
while not rospy.is_shutdown():
InterfaceKeyboard()
def __init__(self):
self.imu_cb_flag = False
self.vicon_cb_flag = False
self.traj_cb_flag = False
self.local_pose_subscriber_prev = 0 # remove
startup_start = timeit.default_timer() # remove
print('start') #
# Rate init
self.rate = rospy.Rate(10.0) # 10 Hz
self.current_state = State()
# Init last_request
self.last_request = rospy.get_rostime() # remove
# vicon yaw
vicon_attitude_sub = rospy.Subscriber("/intel_aero_quad/odom",
Odometry,
self.vicon_attitude_sub_callback)
# IMU yaw
attitude_target_sub = rospy.Subscriber(
"/mavros/imu/data", Imu, self.attitude_target_sub_callback)
traj_yaw_sub = rospy.Subscriber("/px4_quad_controllers/traj_yaw",
PoseStamped,
self.traj_yaw_sub_sub_callback)
self.yaw_target_pub = rospy.Publisher(
"/px4_quad_controllers/yaw_setpoint", PoseStamped, queue_size=10)
while not rospy.is_shutdown():
if (self.vicon_cb_flag and self.imu_cb_flag):
self.vicon_yaw_sp = rospy.get_param(
'/attitude_thrust_publisher/vicon_yaw_sp') # What is this?
if (not self.traj_cb_flag):
self.traj_yaw_sp = 3.14
self.yaw_sp = self.current_yaw_vicon - \
(self.vicon_yaw_sp + self.traj_yaw_sp)
target_yaw = PoseStamped()
target_yaw.header.frame_id = "home"
target_yaw.header.stamp = rospy.Time.now()
target_yaw.pose.position.x = self.current_yaw_imu - self.yaw_sp
self.yaw_target_pub.publish(target_yaw)
# print('VICON setpoint yaw = '+ str(self.vicon_yaw_sp))
# print('VICON current yaw = '+ str(self.current_yaw_vicon))
# print('quad CURRENT yaw = '+ str(self.current_yaw_imu))
# print('final setpoint yaw = \
# '+ str(target_yaw.pose.position.x))
# print('\n')
self.rate.sleep()
def __init__(self):
# Create node with name 'tracker'
rospy.init_node('tracker')
# Initialize instance of CvBridge to convert images between OpenCV images and ROS images
self.bridge = CvBridge()
# A subscriber to the topic '/mavros/local_position/pose. self.pos_sub_cb is called when a message of type 'PoseStamped' is recieved
self.pos_sub = rospy.Subscriber('/mavros/local_position/pose',
PoseStamped, self.pos_sub_cb)
# Quaternion representing the rotation of the drone's body frame in the NED frame. initiallize to identity quaternion
self.quat_bu_lenu = (0, 0, 0, 1)
# A subscriber to the topic '/mavros/state'. self.state_sub_cb is called when a message of type 'State' is recieved
self.state_sub = rospy.Subscriber("/mavros/state", State,
self.state_sub_cb)
# State of the drone.
# self.state.mode = flight mode (e.g. 'OFFBOARD', 'POSCTL', etc),
# self.state.armed = are motors armed (True or False), etc.
self.mode = State().mode
# A subscriber to the topic '/line/param'. self.line_sub_cb is called when a message of type 'Line' is recieved
self.line_sub = rospy.Subscriber('/line/param', LineArray,
self.line_sub_cb)
# A subscriber to the topic '/aero_downward_camera/image'. self.image_sub_cb is called when a message is recieved
self.camera_sub = rospy.Subscriber('/aero_downward_camera/image',
Image, self.camera_sub_cb)
self.image = None
# A publisher which will publish an image annotated with the detected line to the topic 'line/tracker_image'
self.tracker_image_pub = rospy.Publisher('/tracker_image',
Image,
queue_size=1)
# A publisher which will publish the desired linear and anglar velocity to the topic '/setpoint_velocity/cmd_vel_unstamped'
self.velocity_pub = rospy.Publisher('/tracker/vel',
Twist,
queue_size=1)
# Linear setpoint velocities in downward camera frame
self.vx__dc = 0.0
self.vy__dc = 0.0
self.vz__dc = 0.0
# Yaw setpoint velocities in downward camera frame
self.wz__dc = 0.0
self.height = 0.0
# Publishing rate
self.rate = rospy.Rate(_RATE)
# Boolean used to indicate if the streaming thread should be stopped
self.stopped = False
self.error = np.array([0.0, 0.0, 0.0])
self.prev_error = np.array([0.0, 0.0, 0.0])
def __init__(self,
maneuver_velocity_setpoint=np.array([0.1, 0.0, 0.0]),
maneuver_reference_frame='bu',
maneuver_duration=3.0):
""" Object that manages velocity commands for OFFBOARD mode
Attributes:
- vel_sepoint_pub: rospy publisher for cmd_vel_unstamped topic
- vel_setpoint_bu_lenu__lenu: Twist message for desired velocity of quadrotor expressed in local ENU coords in m/s
- state_sub: rospy subscriber for mavros/state topic
- current_state: State() object to access current flight mode
- prev_state: State() object to track most recent flight mode before current
- rate: command rate
- offboard_point_streaming: boolean for if offboard commands should stream or not
- static_transforms: StaticTransforms object to hold relative orientation of different frames (bu, bd, fc, etc)
- maneuver_velocity_setpoint: desired velocity vector for manuever [m/s], related to vel_setpoint_bu_lenu__lenu
- maneuver_reference_frame: frame in which maneuver_velocity_setpoint is expressed
- maneuver_duration: [s] duration of maneuver
"""
# Create node with name 'translation_controller' and set update rate
rospy.init_node('translation_controller')
# A publisher which will publish the desired linear and anglar velocity to the topic '/.../cmd_vel_unstamped'
self.vel_setpoint_pub = rospy.Publisher(
'/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1)
self.vel_setpoint_bu_lenu__lenu = Twist()
# A subscriber to the topic '/mavros/state'. self.state is called when a message of type 'State' is recieved
self.state_sub = rospy.Subscriber("/mavros/state", State,
self.state_cb)
self.current_state = State()
self.prev_state = State()
# A subscriber to the /mavros/local_position/pose topic that is used to access the transform between the body-up
# and local ENU frames
self.pose_sub = rospy.Subscriber('/mavros/local_position/pose',
PoseStamped, self.pose_sub_cb)
self.q_bu_lenu = None
self.rate = rospy.Rate(Constants.RATE)
self.offboard_point_streaming = False
self.static_transforms = StaticTransforms()
self.maneuver_velocity_setpoint = maneuver_velocity_setpoint
self.maneuver_reference_frame = maneuver_reference_frame
self.maneuver_duration = maneuver_duration
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 __init__(self):
def state_cb(state):
self.current_state0 = state
self.current_state0 = State()
self.local_pos_pub0 = rospy.Publisher('uav0/mavros/setpoint_position/local', PoseStamped, queue_size=1)
self.state_sub0= rospy.Subscriber('/uav0/mavros/state', State, state_cb) # $This topic was wrong
self.arming_client0 = rospy.ServiceProxy('/uav0/mavros/cmd/arming', CommandBool)
self.set_mode_client0 = rospy.ServiceProxy('uav0/mavros/set_mode', SetMode)
def __init__(self, ID=0, vehicle_cfg=[]):
# super(Vehicle, self).__init__(*args)
self.ID = ID
self.state = State()
print("Vehicle: ", vehicle_cfg['mavros'])
# ROS services
service_timeout = 10
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service(vehicle_cfg['mavros'] + '/mavros/param/get',
service_timeout)
rospy.wait_for_service(
vehicle_cfg['mavros'] + '/mavros/cmd/arming', service_timeout)
rospy.wait_for_service(
vehicle_cfg['mavros'] + '/mavros/mission/push',
service_timeout)
rospy.wait_for_service(
vehicle_cfg['mavros'] + '/mavros/mission/clear',
service_timeout)
rospy.wait_for_service(vehicle_cfg['mavros'] + '/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(
vehicle_cfg['mavros'] + '/mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy(
vehicle_cfg['mavros'] + '/mavros/cmd/arming', CommandBool)
self.set_mode_srv = rospy.ServiceProxy(
vehicle_cfg['mavros'] + '/mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy(
vehicle_cfg['mavros'] + '/mavros/mission/clear', WaypointClear)
self.wp_push_srv = rospy.ServiceProxy(
vehicle_cfg['mavros'] + '/mavros/mission/push', WaypointPush)
# self.state_sub = rospy.Subscriber(vehicle_cfg['mavros'] + '/mavros/state',
# State,
# self.state_callback)
# # ROS subscribers
# self.sub_topics_ready = {
# key: False
# for key in [
# 'state'
# ]
# }
# send setpoints in seperate thread to better prevent failsafe
self.thread = Thread(target=self.activate, args=())
self.thread.daemon = True
def setUp(self):
self.rate = rospy.Rate(10) # 10hz
self.has_global_pos = False
self.global_position = NavSatFix()
self.extended_state = ExtendedState()
self.altitude = Altitude()
self.state = State()
self.mc_rad = 5
self.fw_rad = 60
self.fw_alt_rad = 10
self.last_alt_d = None
self.last_pos_d = None
self.mission_name = ""
self.sub_topics_ready = {
key: False
for key in ['global_pos', 'home_pos', 'ext_state', 'alt', 'state']
}
# setup ROS topics and services
try:
rospy.wait_for_service('mavros/mission/push', 30)
rospy.wait_for_service('mavros/cmd/arming', 30)
rospy.wait_for_service('mavros/set_mode', 30)
except rospy.ROSException:
self.fail("failed to connect to mavros services")
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
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.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
# need to simulate heartbeat to prevent datalink loss detection
self.hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(
mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0)
self.hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1))
self.hb_ros_msg = mavlink.convert_to_rosmsg(self.hb_mav_msg)
self.hb_thread = Thread(target=self.send_heartbeat, args=())
self.hb_thread.daemon = True
self.hb_thread.start()
def __init__(self):
self.rate = rospy.Rate(20)
self.current_state = State()
rospy.Subscriber('/mavros/state', State, self.state_cb)
while not rospy.is_shutdown() and not self.current_state.connected:
self.rate.sleep()
rospy.loginfo('Connection Successful')
self.local_pose = PoseStamped()
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self.pose_cb)
