def __init__(self): print("[INFO] initializing config...") self.publish_images = True self.bridge = CvBridge() print("[INFO] Loading ROS topics...") self.bboxes_pub = rospy.Publisher( "/yolo/Segbboxes", Detection2DArray, queue_size=1) #Bboxes for object_handler self.images_pub = rospy.Publisher( "/yolo/Segimages", Detection2DArray, queue_size=1) #Bboxes with images for display cloud_sub = message_filters.Subscriber( "/zed2/zed_node/point_cloud/cloud_registered", PointCloud2) self.cache = message_filters.Cache(cloud_sub, 30) #boxes_sub = message_filters.Subscriber("/yolo/bboxes",Detection2DArray) #rospy.Subscriber("/zed2/zed_node/point_cloud/cloud_registered",PointCloud2, self.callback_cloud, queue_size = 30) rospy.Subscriber("/zed2/zed_node/point_cloud/cloud_registered", PointCloud2, self.callback_cloud, queue_size=1) rospy.Subscriber("/yolo/bboxes", Detection2DArray, self.callback_box, queue_size=1) print("[INFO] Loading complete") self.cloud_list = [] self.cloud_list_headers = [] self.queue_lim = 60
def __init__(self, ip, joy_topic="/cmd_vel", scale = 0.005): self.robot = robot_controller.Ur3(ip, 30003, 30002) self.sub = message_filters.Subscriber(joy_topic, Twist) self.cache = message_filters.Cache(self.sub, cache_size=1, allow_headerless=True) self.scale = scale self.last_pose = self.robot.get_pose() self.bounds_x = (0.225, -0.225) self.bounds_y = (0.315, -0.315)
def __init__(self, name): self.name = name self.tf = '/vicon/'+name+'/'+name self.tl = TransformListener() self.pose = np.array([0,0,0]) self.orient = np.array([0,0,0]) # for velocity: sub = message_filters.Subscriber(self.tf, TransformStamped) self.cache = message_filters.Cache(sub, 100) self.vel = np.array([0,0,0])
def listener(): rospy.init_node('numpy_listener') #rospy.Subscriber("/mobile_base/sensors/imu_data", Imu, imu_callback) #rospy.Subscriber("/odom", Odometry, odom_callback) # rospy.Subscriber("/map", OccupancyGrid, map_callback) #odom_init_once = None #odom_init_once = rospy.Subscriber("/odom", Odometry, odom_init_once) rospy.Subscriber("/scan", LaserScan, laser_callback) # rospy.Subscriber("floats", Floats, callback) # Time sync problem ... it seems that computer is too slow to process, # so the algorithm runs with latest state but older measurements! odom_sub = message_filters.Subscriber("/odom", Odometry) global odom_cache odom_cache = message_filters.Cache(odom_sub, 2) scan_sub = message_filters.Subscriber("/scan", LaserScan) global laser_cache laser_cache = message_filters.Cache(scan_sub, 2) ts = message_filters.TimeSynchronizer([odom_sub, scan_sub], 10) #ts.registerCallback(laser_callback, 1) rospy.spin()
def __init__(self, name, leader=False): Mocap_object.__init__(self, name) self.leader = leader self.sp = np.array([0., 0., 0.]) self.near_obstacle = False self.nearest_obstacle = None self.rad_imp = radius_impedance_model() # Obstacle avoidance sub_sp = message_filters.Subscriber(self.name + "_sp", PoseStamped) self.cache_sp = message_filters.Cache(sub_sp, 100) self.vel_sp = np.array([0, 0, 0])
def __init__(self, template_file_list_name, subscribe_topic_name, model_file, dlib_face_predictor): self.recognizer = face_recognizer.face_recognizer( model_file, dlib_face_predictor) if not self.recognizer.set_inside_templates(template_file_list_name): print "fail" exit() self.bridge = CvBridge() sub = message_filters.Subscriber(subscribe_topic_name, sensorImg) self.cache = message_filters.Cache(sub, 100) thread.start_new_thread(self.run, ())
def main(): rospy.init_node('segment_bag_to_video') if len(sys.argv) < 3: print 'Usage: segment_bag_to_video.py model.ckpt INPUT.bag OUTPUT.bag' return checkpoint_path = sys.argv[1] hand_segmentation = HandSegmentation(checkpoint_path) input_path = sys.argv[2] output_path = sys.argv[3] input_bag = rosbag.Bag(input_path) output_bag = rosbag.Bag(output_path, 'w') color_pass_to_cache = PassToCache() depth_pass_to_cache = PassToCache() color_cache = message_filters.Cache(color_pass_to_cache, cache_size=5) depth_cache = message_filters.Cache(depth_pass_to_cache, cache_size=5) demo = Demo(hand_segmentation, output_bag) queue_size = 1 slop_seconds = 0.015 sync = message_filters.ApproximateTimeSynchronizer( [color_cache, depth_cache], queue_size, slop_seconds) sync.registerCallback(demo.callback) i = 0 message_count = input_bag.get_message_count(topic_filters=[COLOR_TOPIC, DEPTH_TOPIC]) for topic, msg, t in input_bag.read_messages(topics=[COLOR_TOPIC, DEPTH_TOPIC]): i += 1 if i % 100 == 0: print 'Processed {} of {} messages ({}%)'.format(i, message_count, 100.0 * float(i) / message_count) if topic == COLOR_TOPIC: color_cache.add(msg) elif topic == DEPTH_TOPIC: depth_cache.add(msg) output_bag.close()
def __init__(self, robot_ip, joy_topic="/joy", gripper_topic="/gripper/command", scale=0.001): print('Connecting to {}'.format(robot_ip)) self.robot = robot_controller.Ur3(robot_ip, 30003, 30002) self.sub = message_filters.Subscriber(joy_topic, Joy) self.cache = message_filters.Cache(self.sub, cache_size=1, allow_headerless=False) self.pub = rospy.Publisher(gripper_topic, String, queue_size=2) self.scale = scale self.orient_scale = 0.01 self.last_pose = self.robot.get_pose() self.last_data_h_seq = None self.dead_zone = 0.2 self.update_count = 20 self.update_counter = 0
def setupMessageInterface(self): """ Setup ROS publisher, subscriber, and message filters """ self.__unlock() # Topic Names drtkTopic = rospy.get_param('~drtk_topic', "/drtk_node/drtk_output") tdcpLocalTopic = rospy.get_param('~tdcp_local_topic', "/tdcp_node/tdcp_output") # Message Interface Parameters queueSize = rospy.get_param('~queue_size', 10) cacheSize = rospy.get_param('~cache_size', 10) self.maxTimeOffset = rospy.get_param('~max_time_offset', 0.25) # Publishers self.path_pub = rospy.Publisher("~path", PathDiagnostics, queue_size=queueSize) # ROS Message Filters drtk_sub = message_filters.Subscriber(drtkTopic, DrtkOutput) # Message Filter Caches self.drtk_cache = message_filters.Cache(drtk_sub, cache_size=cacheSize) # Sync Variables self.latestTdcpLocalMsg = TdcpOutput() self.newTdcpLocalFlag = 0 # ROS Subscriber tdcp_local_sub = rospy.Subscriber(tdcpLocalTopic, TdcpOutput, self.tdcpLocalCallback, queue_size=queueSize) # ROS Timer rospy.Timer(rospy.Duration(0.05), self.tick) self.stopwatch = 0.0
def __init__(self, config): self.config = config self.pp = pprint.PrettyPrinter(indent=4) self.pp.pprint(config) rgb_image_sub = message_filters.Subscriber( "/camera/rgb/image_color", ImageSensor_msg, queue_size=10000 ) rgb_info_sub = message_filters.Subscriber( "/camera/rgb/camera_info", CameraInfo ) depth_info_sub = message_filters.Subscriber( "/camera/depth/camera_info", CameraInfo ) depth_image_sub = message_filters.Subscriber( "/camera/depth/image", ImageSensor_msg, queue_size=10000 ) self.depth_scale = 100.0 self.depth_image_cache = message_filters.Cache(depth_image_sub, 10000, allow_headerless=False) self.cv_bridge = CvBridge() self.output_path = config['label_gen']['output_path'] model_path = get_model_path(config['label_gen']['model_dir'], config['label_gen']['object_name'], model_type="upright") self.initial_rotation = get_initial_rotation(config['label_gen']['object_label']) # model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/004_sugar_box/textured.ply" # self.output_path = "./bag_output/sugar_1" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, -math.pi/2) #sugar_1 # self.output_path = "./bag_output/sugar_2" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, -2.0 * math.pi/3) #sugar_2 # self.output_path = "./bag_output/sugar_3" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.0 * math.pi/3) #sugar_3 # model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/035_power_drill/textured_upright.ply" # self.output_path = "./bag_output/drill_1" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, math.pi) #drill_1 # self.output_path = "./bag_output/drill_2" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.5/3 * math.pi) #drill_2 # self.output_path = "./bag_output/drill_3" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, -2.5/3 * math.pi) #drill_3 # model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/005_tomato_soup_can/textured.ply" # self.output_path = "./bag_output/soup_1" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 0) #soup_1 # model_path = "/media/aditya/A69AFABA9AFA85D9/Datasets/YCB_Video_Dataset/models/006_mustard_bottle/textured.ply" # self.output_path = "./bag_output/mustard_1" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 0) #mustard_1 # self.output_path = "./bag_output/mustard_2" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.5/3 * math.pi) #mustard_2 # self.output_path = "./bag_output/sugar_3" # self.initial_rotation = tf.transformations.quaternion_from_euler(0, 0, 2.0 * math.pi/3) #sugar_3 mkdir_if_missing(self.output_path) self.world_frame = "/base_footprint" self.camera_frame = "/camera_rgb_optical_frame" self.tf_listener = tf.TransformListener() self.camera_pose = None self.rgb_camera_instrinc_matrix = None self.depth_camera_instrinc_matrix = None self.counter = 0 self.MAX_COUNTER = 400 self.pub_filtered_cloud = \ rospy.Publisher( "image_node/filtered_cloud", PointCloud2, queue_size=10 ) self.pub_pose_cloud = \ rospy.Publisher( "image_node/pose_cloud", PointCloud2, queue_size=10 ) # Read Model cloud = PlyData.read(model_path).elements[0].data cloud = np.transpose(np.vstack((cloud['x'], cloud['y'], cloud['z']))) cloud_pose = pcl.PointCloud() cloud_pose.from_array(cloud) sor = cloud_pose.make_voxel_grid_filter() sor.set_leaf_size(0.015, 0.015, 0.015) cloud_pose = sor.filter() self.mesh_cloud = np.asarray(cloud_pose) # Z point up self.object_height = np.max(self.mesh_cloud[:,2]) - np.min(self.mesh_cloud[:,2]) self.cloud_filter_params = { "xmin" : 0.1, "xmax" : 0.6, "ymin" : 0.0, "ymax" : 1.7, "zmin" : 0.75, #drill, sugar, mustard # "zmin" : 0.76, #soup can "object_height" : self.object_height, "downsampling_leaf_size" : 0.015 } print(self.cloud_filter_params) print ("Num points after downsample and filter : {}".format( self.mesh_cloud.shape[0])) rgb_image_sub.registerCallback(self.image_callback) rgb_info_sub.registerCallback(self.rgb_info_callback) depth_info_sub.registerCallback(self.depth_info_callback)
import tf import struct from laser_assembler.srv import AssembleScans from laser_geometry import LaserProjection from sensor_msgs.msg import PointCloud, ChannelFloat32, Image, CameraInfo, LaserScan from sensor_msgs import point_cloud2 from image_geometry import PinholeCameraModel from cv_bridge import CvBridge, CvBridgeError from geometry_msgs.msg import PointStamped, Point32 rospy.init_node('camera_lidar') tf_listener = tf.TransformListener() image_sub = message_filters.Subscriber('camera/image_raw', Image) image_cache = message_filters.Cache(image_sub, 10) camera_model = PinholeCameraModel() bridge = CvBridge() def on_camera_info(camera_info): camera_model.fromCameraInfo(camera_info) camera_info_sub = rospy.Subscriber('camera/camera_info', CameraInfo, on_camera_info, queue_size=1) laser_projector = LaserProjection()
def __init__(self, name, leader=False): Mocap_object.__init__(self, name) self.sp = np.array([0., 0., 0.]) sub_sp = message_filters.Subscriber(self.name + "_sp", PoseStamped) self.cache_sp = message_filters.Cache(sub_sp, 100) self.vel_sp = np.array([0, 0, 0])
def __init__(self, **kwargs): """ Initializes a new FireflyEnv environment. To check any topic we need to have the simulations running, we need to do two things: 1) Unpause the simulation: without that the stream of data doesnt flow. This is for simulations that are pause for whatever the reason 2) If the simulation was running already for some reason, we need to reset the controllers. This has to do with the fact that some plugins with tf, dont understand the reset of the simulation and need to be reseted to work properly. The Sensors: The sensors accesible are the ones considered usefull for AI learning. Sensor Topic List: * /pose : Reads the estimated pose of the MAV. Type: uav_msgs * /target_tracker/pose : Reads the fused estimate of the target person. Type: geometry_msgs Actuators Topic List: * /command, publishes the desired waypoints and velocity for the robot. Type: uav_msgs Args: """ rospy.logdebug("Start FireflyEnv INIT...") rospy.logwarn('Setting Up Firefly ENV') # Variables that we give through the constructor. # None in this case # Internal Vars # Doesnt have any accesibles self.controllers_list = [] # It doesnt use namespace self.robot_name_space = "" # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv super(FireflyMultiAgentGTEnv, self).__init__(controllers_list=self.controllers_list, robot_name_space=self.robot_name_space, reset_controls=False, start_init_physics_parameters=False, reset_world_or_sim="WORLD", **kwargs) machine_name = '/machine_' + str(self.robotID) rotors_machine_name = '/firefly_' + str(self.robotID) self.rotors_machine_name = rotors_machine_name self.num_robots = 2 if self.robotID == self.num_robots: neighbor_name = '/machine_' + str( (self.robotID + 1) % self.num_robots) rotors_neighbor_name = '/firefly_' + str( (self.robotID + 1) % self.num_robots) else: neighbor_name = '/machine_' + str(self.robotID + 1) rotors_neighbor_name = '/firefly_' + str(self.robotID + 1) self.rotors_neighbor_name = rotors_neighbor_name self.pose_topic = machine_name + "/pose" self.velocity_topic = rotors_machine_name + "/ground_truth/odometry" self.velocity_neighbor_topic = rotors_neighbor_name + "/ground_truth/odometry" self.gt_pose_topic = machine_name + "/pose/groundtruth" self.gt_neighbor_pose_topic = neighbor_name + "/pose/groundtruth" self.firefly_pose_topic = rotors_machine_name + "/ground_truth/pose_with_covariance" self.target_topic = machine_name + "/target_tracker/pose" self.gt_target_topic = "/actorpose" self.gt_target_vel_topic = "/actorvel" self.target_velocity_topic = machine_name + "/target_tracker/twist" self.command_topic = machine_name + "/command" self.neighbor_command_topic = neighbor_name + "/command" self.destination_topic = machine_name + "/destination" self.detections_feedback_topic = machine_name + "/object_detections/feedback" self.noisy_joints_topic = machine_name + "/noisy_joints" self.noisy_joints_neighbor_topic = neighbor_name + "/noisy_joints" self.noisy_bbox_topic = machine_name + "/noisy_bbox" self.noisy_bbox_neighbor_topic = neighbor_name + "/noisy_bbox" self.noisy_detection = machine_name + "/noisy_detection" self.noisy_neighbor_detection = neighbor_name + "/noisy_detection" self.alphapose_topic = machine_name + "/result_alpha" self.alphapose_neighbor_topic = neighbor_name + "/result_alpha" self.alphapose_bbox_topic = machine_name + "/result_bbox" self.alphapose_bbox_neighbor_topic = neighbor_name + "/result_bbox" self.alphapose_detection = machine_name + "/detection" self.alphapose_neighbor_detection = neighbor_name + "/detection" self.mhmr_topic = "/multihmr_joints" self.camera_info = rotors_machine_name + rotors_machine_name + "/xtion/rgb/camera_info" self.camera_info_neighbor = rotors_neighbor_name + rotors_neighbor_name + "/xtion/rgb/camera_info" self.joints_gt = '/gt_joints' self.mpc_command = machine_name + "/command_MPC" import tf self.listener = tf.TransformListener() self.gazebo.unpauseSim() #self.controllers_object.reset_controllers() # We Start all the ROS related Subscribers and publishers rospy.Subscriber(self.firefly_pose_topic, PoseWithCovarianceStamped, self._firefly_pose_callback) rospy.Subscriber(self.pose_topic, uav_pose, self._pose_callback) velocity = message_filters.Subscriber(self.velocity_topic, Odometry) self.velocity_cache = message_filters.Cache(velocity, 300) velocity_neighbor = message_filters.Subscriber( self.velocity_neighbor_topic, Odometry) self.velocity_neighbor_cache = message_filters.Cache( velocity_neighbor, 300) rospy.Subscriber(self.gt_pose_topic, uav_pose, self._gt_pose_callback) rospy.Subscriber(self.gt_neighbor_pose_topic, uav_pose, self._gt_neighbor_pose_callback) # self.gt_neighbor_cache = message_filters.Cache(gt_neighbor,300); rospy.Subscriber(self.target_topic, PoseWithCovarianceStamped, self._target_callback) gt_target = message_filters.Subscriber(self.gt_target_topic, Odometry) self.gt_target_cache = message_filters.Cache(gt_target, 300) rospy.Subscriber(self.target_velocity_topic, TwistWithCovarianceStamped, self._target_vel_callback) rospy.Subscriber(self.detections_feedback_topic, NeuralNetworkFeedback, self._detections_feedback_callback) noisy_joints = message_filters.Subscriber(self.noisy_joints_topic, AlphaRes) self.noisy_joints_cache = message_filters.Cache(noisy_joints, 300) self.noisy_joints_cache.registerCallback(self._noisy_joints_callback) noisy_joints_neighbor = message_filters.Subscriber( self.noisy_joints_neighbor_topic, AlphaRes) self.noisy_joints_neighbor_cache = message_filters.Cache( noisy_joints_neighbor, 300) self.noisy_joints_neighbor_cache.registerCallback( self._noisy_joints_neighbor_callback) noisy_bbox = message_filters.Subscriber(self.noisy_bbox_topic, AlphaRes) self.noisy_bbox_cache = message_filters.Cache(noisy_bbox, 300) self.noisy_bbox_cache.registerCallback(self._noisy_bbox_callback) noisy_bbox_neighbor = message_filters.Subscriber( self.noisy_bbox_neighbor_topic, AlphaRes) self.noisy_bbox_neighbor_cache = message_filters.Cache( noisy_bbox_neighbor, 300) self.noisy_bbox_neighbor_cache.registerCallback( self._noisy_bbox_neighbor_callback) rospy.Subscriber(self.noisy_detection, Int16, self._noisy_detection_callback) rospy.Subscriber(self.noisy_neighbor_detection, Int16, self._noisy_neighbor_detection_callback) alphapose = message_filters.Subscriber(self.alphapose_topic, AlphaRes) self.alphapose_cache = message_filters.Cache(alphapose, 300) self.alphapose_cache.registerCallback(self._alphapose_callback) alphapose_neighbor = message_filters.Subscriber( self.alphapose_neighbor_topic, AlphaRes) self.alphapose_neighbor_cache = message_filters.Cache( alphapose_neighbor, 100) self.alphapose_neighbor_cache.registerCallback( self._alphapose_neighbor_callback) alphapose_bbox = message_filters.Subscriber(self.alphapose_bbox_topic, AlphaRes) self.alphapose_bbox_cache = message_filters.Cache(alphapose_bbox, 300) alphapose_bbox_neighbor = message_filters.Subscriber( self.alphapose_bbox_neighbor_topic, AlphaRes) self.alphapose_bbox_neighbor_cache = message_filters.Cache( alphapose_bbox_neighbor, 300) mhmr = message_filters.Subscriber(self.mhmr_topic, PoseArray) self.mhmr_cache = message_filters.Cache(mhmr, 300) rospy.Subscriber(self.alphapose_detection, Int16, self._alphapose_detection_callback) rospy.Subscriber(self.alphapose_neighbor_detection, Int16, self._alphapose_neighbor_detection_callback) rospy.Subscriber(self.command_topic, uav_pose, self._command_callback) rospy.Subscriber(self.camera_info, CameraInfo, self._camera_info_callback) rospy.Subscriber(self.camera_info_neighbor, CameraInfo, self._camera_info_neighbor_callback) rospy.Subscriber(self.joints_gt, PoseArray, self._joints_gt_callback) rospy.Subscriber(self.mpc_command, uav_pose, self._mpc_command_callback) self._dest_vel_pub = rospy.Publisher(self.destination_topic, PoseStamped, queue_size=1) self._cmd_vel_pub = rospy.Publisher(self.command_topic, uav_pose, queue_size=1) self._cmd_neighbor_vel_pub = rospy.Publisher( self.neighbor_command_topic, uav_pose, queue_size=1) self._target_init_pub = rospy.Publisher(self.target_topic, PoseWithCovarianceStamped, queue_size=1) self.create_circle(radius=8) # self.gazebo.pauseSim() outPose = uav_pose() outPose.header.stamp = rospy.Time.now() outPose.header.frame_id = "world" outPose.POI.x = 0 outPose.POI.y = 0 outPose.POI.z = 0 r = 8 # t = np.random.choice(63, 1) outPose.position.x = r * np.cos(self.theta[t[0]]) outPose.position.y = r * np.sin(self.theta[t[0]]) outPose.position.z = -r self._cmd_vel_pub.publish(outPose) rospy.logwarn("Finished FireflyEnv INIT...") self._check_all_sensors_ready() rospy.logwarn("SENSORS OK!.") self._check_publishers_connection() rospy.logwarn("PUBLISHERS OK!.")
#the message filters below are used to cache the messages published on each topic. This is necessary because the #camera capture packages publish the number of cars detected on a frame to frame basis. In an instance #where the vehicled detector missess detecting a vehicle for a few frames(false negatives), the subscriber might #get car count of 0, indicating no vehicles on a lane which is not correct. Therefore, using a cache, we take the #average number of cars detected in 5(or any number) of frames. This way, we increase the tolerance of our vehicle #detector. #In all the camera capture packages and this one, we have changed the message type of the lane counters to 'stampedInt'. #stampedInt is a custom message type i've created in the ros_arduino_msgs folder which is basically an Integer that #also has a time stamp. The time stamp is required to use the message filter's caching capability. lane1 = message_filters.Subscriber("/lane1_cars", stampedInt) cache = message_filters.Cache( lane1, 5 ) #Change number to change the cache size. A value of 1 represents the same thing as no caching. cache.registerCallback(lane1_callback) lane2 = message_filters.Subscriber("/lane2_cars", stampedInt) cache2 = message_filters.Cache(lane2, 5) cache2.registerCallback(lane2_callback) lane3 = message_filters.Subscriber("/lane3_cars", stampedInt) cache3 = message_filters.Cache(lane3, 5) cache3.registerCallback(lane3_callback) lane4 = message_filters.Subscriber("/lane4_cars", stampedInt) cache4 = message_filters.Cache(lane4, 5) cache4.registerCallback(lane4_callback)