def publish(anns, data):
ar_mk_list = AlvarMarkers()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# AR markers
object = deserialize_msg(d.data, AlvarMarker)
ar_mk_list.markers.append(object)
# Visual markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "ar_mk_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('ar_mk_marker', MarkerArray, latch=True, queue_size=1)
ar_mk_pub = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)
ar_mk_pub.publish(ar_mk_list)
marker_pub.publish(marker_list)
return
def __init__(self):
self.found = False
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.arCallback)
self.ar_placeholder = AlvarMarkers()
#Id of the marker
self.id = 0
#Distance from the ar tag
self.distance = 1000
def ar_pose_cb(self,msg):
self.poses_msg = AlvarMarkers()
self.poses_msg.markers = [self.process_marker(i) for i in msg.markers]
if len(self.poses_msg.markers) > 0:
if _DEBUG: rospy.loginfo("publishing poses!")
self.ar_pose_pub.publish(self.poses_msg)
def __init__(self, publisher):
self.data = AlvarMarkers()
self.data.markers = [AlvarMarker(), AlvarMarker()]
self.pub = publisher
self.data.markers[0].id = 1
self.data.markers[1].id = 2
self.data.header.frame_id = 'base_footprint'
self.last_update = rospy.Time.now()
def ar_pose_cb(self, msg):
markers = msg.markers # an array of AlvarMarkers
def pose_filter(marker):
if marker.pose.pose.position.z > _FILTER_DIST: # z can be considered the same as body up x
return False # it's not a good tag because it's too far
return True # it's a good tag because it's close enough
filtered_markers = filter(
pose_filter, markers) # calls pose_filter on every item in markers
if len(filtered_markers) < 1:
return # if no valid markers, don't publish anything
output_message = AlvarMarkers(
) # initalizes a new AlvarMarkers message
output_message.markers = filtered_markers # sets the array of markers to the filtered list
self.ar_pose_pub.publish(
output_message) # publishes the filtered message
def sub_cb(self, msg):
marker_list = msg.markers
tmp = AlvarMarkers()
for alvar_marker in marker_list:
pose_msg = PoseWithCovarianceStamped()
pose_msg.header.stamp = rospy.Time.now()
if alvar_marker.id in self.ar_id:
pose_msg.pose.pose = alvar_marker.pose.pose
pose_msg.header.frame_id = str(alvar_marker.id)
self.pub.publish(pose_msg)
def __init__(self, args):
self.node_name = rospy.get_name().replace('/', '')
self.desired_freq = args['desired_freq']
self._frame_id = args['frame_id']
self._base_frame_id = args['base_frame_id']
self._publish_saved_markers_tf = args['publish_saved_markers_tf']
self._folder_path = args['folder_path']
self._markers_filename = args['markers_filename']
self._load_markers_on_init = args['load_markers_on_init']
self._max_marker_id = args['max_marker_id']
self._min_marker_id = args['min_marker_id']
# Checks value of freq
if self.desired_freq <= 0.0 or self.desired_freq > MAX_FREQ:
rospy.loginfo(
'%s::init: Desired freq (%f) is not possible. Setting desired_freq to %f'
% (self.node_name, self.desired_freq, DEFAULT_FREQ))
self.desired_freq = DEFAULT_FREQ
self.real_freq = 0.0
# Saves the state of the component
self.state = State.INIT_STATE
# Saves the previous state
self.previous_state = State.INIT_STATE
# flag to control the initialization of the component
self.initialized = False
# flag to control the initialization of ROS stuff
self.ros_initialized = False
# flag to control that the control loop is running
self.running = False
# Variable used to control the loop frequency
self.time_sleep = 1.0 / self.desired_freq
# State msg to publish
self.msg_state = MarkerMappingState()
# Timer to publish state
self.publish_state_timer = 1
# Saves the time with
self.last_marker_time = rospy.Time.now()
# Saves the received markers
self._marker_msg = AlvarMarkers()
# Dict to save the markers
self._marker_dict = {}
# The covariance set whenever setting the global pose
self._default_pose_covariance = [
0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001
]
self.t_publish_state = threading.Timer(self.publish_state_timer,
self.publishROSstate)
def __init__(self):
smach.State.__init__(self, outcomes=['to_menu'])
self.arduino_publisher = rospy.Publisher('/toggle_relay',
Bool,
queue_size=10)
self.electromagnet_state = Bool()
self.artag_subscriber = rospy.Subscriber('/ar_pose_marker',
AlvarMarkers,
self.artag_callback)
self.ar_tag_markers = AlvarMarkers()
def publish(markers):
ar_list = AlvarMarkers()
for m in markers:
ar = AlvarMarker()
ar.id = m['id']
ar.confidence = m['confidence']
ar.pose.header.frame_id = m['frame_id']
ar.pose.header.stamp = rospy.Time.now()
ar.pose.pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/Pose', m['pose'])
ar_list.markers.append(ar)
ar_pub.publish(ar_list)
return
def __init__(self, maze_pub_topic, img_sub_topic, cam_info_topic,
cam_frame, base_frame, wall_color, turtlebot_color, goal_id,
marker_id, maze_rows, maze_cols, ar_size):
# message handling
self.messages = deque([], 5)
self.maze_pub = rospy.Publisher(maze_pub_topic, Maze, queue_size=10)
self.maze = Maze()
self._bridge = CvBridge()
# intialize tf listener
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
# get frame information
self.goal_id = goal_id
self.goal_frame = 'ar_marker_' + str(goal_id)
self.marker_id = marker_id
self.marker_frame = 'ar_marker_' + str(marker_id)
self.turtlebot_color = turtlebot_color
self.cam_frame = cam_frame
self.base_frame = base_frame
self.wall_color = wall_color
# counter for number of callbacks
self.num_steps = 0
#AR Marker attributes
self.visible_markers = AlvarMarkers()
self.ar_size = ar_size
#Median Filter objects (noise reduction)
self.corner_tracker = NoiseTracker(tolerance=10, queue_size=20)
self.turtlebot_tracker = NoiseTracker(tolerance=30, queue_size=7)
#Grid object for handling updates
self.grid = Grid(maze_rows, maze_cols)
# initialize queue for image and camera sync
image_sub = message_filters.Subscriber(img_sub_topic, Image)
caminfo_sub = message_filters.Subscriber(cam_info_topic, CameraInfo)
ts = message_filters.ApproximateTimeSynchronizer(
[image_sub, caminfo_sub],
queue_size=100,
slop=0.1,
allow_headerless=True)
ts.registerCallback(self.callback)
offb_set_mode = SetMode()
global sp
sp = PoseStamped()
# callback method for state sub
def state_cb(state):
global current_state
current_state = state
# callback method for local pos sub
def locpos_cb(locpos):
global lp
lp = locpos
# callback method for marker local position
ardata = AlvarMarkers()
def marker_cb(data):
global ardata
ardata = data
local_pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, locpos_cb)
local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
state_sub = rospy.Subscriber('/mavros/state', State, state_cb)
arming_client = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)
def trajgen(mode='setpoint_relative',number_of_points=50, x_des=0,y_des=0,z_des=0):
if mode=='line':
# line trajectory
x = np.hstack( [np.linspace(0, -0.5, number_of_points), np.linspace(-0.5, 0, number_of_points)] )
y = np.zeros(2*number_of_points)
#!/usr/bin/env python
import rospy
import cv2
from ar_track_alvar_msgs.msg import AlvarMarkers
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker
from std_msgs.msg import Int8
# Create message variables
artag = AlvarMarkers()
sendpnt = Point()
newmarker = Marker()
class ar2pnt:
# Initialize Class
def __init__(self):
self.pnt = []
self.calb = 0
self.bridge = CvBridge()
self.status = 0
self.image_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.imagecb)
self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.arCb)
self.marker = rospy.Subscriber('/visualization_marker', Marker, self.markerCb)
self.pub = rospy.Publisher("ar_point",Point,queue_size=10)
self.pubmarker = rospy.Publisher('/visualization_marker_update',Marker,queue_size=10)
self.statsub = rospy.Subscriber('robot_state',Int8,self.stateCb)
"/track_position_line/track_ar_colors_param",
"[0.3,0.5,0.3,1];[1,0,0,1]").split(";")
ros_rate_param = rospy.get_param("/track_position_line/ros_rate_param", 10)
queue_size_param = rospy.get_param("/track_position_line/queue_size_param",
100)
considered_displacement = rospy.get_param(
"/track_position_line/considered_displacement", 0.02)
output_topic = rospy.get_param("/track_position_line/output_topic",
"track_position_line")
input_topic = rospy.get_param("/track_position_line/input_topic",
"/ar_pose_marker")
output_frame = rospy.get_param("/track_position_line/output_frame", "usb_cam")
line_scale = rospy.get_param("/track_position_line/line_scale", 0.01)
#Global variables
arData = AlvarMarkers()
marker_array = MarkerArray()
info_array = infoVector()
ar_pose_matrix = numpy.full((len(track_ar_ids_param), int(queue_size_param)),
Point())
ar_color_rgba = numpy.full(len(track_ar_ids_param), ColorRGBA())
matrix_position = numpy.zeros(len(track_ar_ids_param), int)
pub_rviz_markerArray = rospy.Publisher(output_topic,
MarkerArray,
queue_size=10)
pub_traveled = rospy.Publisher("/total_traveled", infoVector, queue_size=10)
#configuring the rgb colors of the line
import rospy
import tf
from ar_track_alvar_msgs.msg import AlvarMarkers
from geometry_msgs.msg import PoseStamped
from math import *
import message_filters
from sensor_msgs.msg import Imu
from visualization_msgs.msg import Marker
#data_to_publish = PoseStamped()
#pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
data_marker = AlvarMarkers()
data_imu = Imu()
data_to_publish = PoseStamped()
pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
def callback_imu(data):
global data_imu
data_imu = data
def callback_marker(data):
data_marker = data
rospy.Subscriber('mavros/imu/data', Imu, callback_imu)
from sensor_msgs.msg import Imu from geometry_msgs.msg import Twist from geometry_msgs.msg import Vector3 from nav_msgs.msg import Odometry from ardrone_autonomy.msg import Navdata from ar_track_alvar_msgs.msg import AlvarMarkers, AlvarMarker from drone import Drone #Variables initialized cmd_vel = Twist() nav_data = Navdata() imu_data = Imu() odom = Odometry() empty = Empty() ar_data = AlvarMarkers() # queues for keeping center positions to take average mean_center_x = collections.deque(maxlen=20) mean_center_y = collections.deque(maxlen=20) mean_center_z = collections.deque(maxlen=20) mean_center_yaw = collections.deque(maxlen=20) # TODO set it to false if you want to fly the drone testing = True cx = 0.0 cy = 0.0 cz = 0.0 markerFlag = 0 # if we see the marker statusFlag = 1
