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