command.linear.x = 5.5 else: command.linear.x = 0 wayPoint = goalPos print(resetWp) print 'Current Angle: {0}'.format(wayPoint) pub.publish(command) # main function call if __name__ == "__main__": # Initialize the node rospy.init_node('lab2', log_level=rospy.DEBUG) # subscribe to laser scan message sub = message_filters.Subscriber('base_scan', LaserScan) # subscribe to odometry message sub2 = message_filters.Subscriber('odom', Odometry) # synchronize laser scan and odometry data ts = message_filters.TimeSynchronizer([sub, sub2], 10) ts.registerCallback(callback) # publish twist message pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) # Turn control over to ROS rospy.spin()
def init(): global flags global underwater_image_pub global underwater_camera_info_pub flags = tf.app.flags flags.DEFINE_integer("epoch", 1, "Epoch to train [25]") flags.DEFINE_float("learning_rate", 0.0002, "Learning rate of for adam [0.0002]") flags.DEFINE_float("beta1", 0.5, "Momentum term of adam [0.5]") flags.DEFINE_float("train_size", np.inf, "The size of train images [np.inf]") flags.DEFINE_integer("batch_size", 64, "The size of batch images [64]") flags.DEFINE_integer( "input_height", 480, "The size of image to use (will be center cropped). [108]") flags.DEFINE_integer( "input_width", 640, "The size of image to use (will be center cropped). If None, same value as input_height [None]" ) flags.DEFINE_integer( "input_water_height", 1024, "The size of image to use (will be center cropped). [108]") flags.DEFINE_integer( "input_water_width", 1360, "The size of image to use (will be center cropped). If None, same value as input_height [None]" ) flags.DEFINE_integer("output_height", 48, "The size of the output images to produce [64]") flags.DEFINE_integer( "output_width", 64, "The size of the output images to produce. If None, same value as output_height [None]" ) flags.DEFINE_integer("c_dim", 3, "Dimension of image color. [3]") flags.DEFINE_float("max_depth", 1.5, "Dimension of image color. [3.0]") flags.DEFINE_string("water_dataset", "water_images", "The name of dataset [celebA, mnist, lsun]") flags.DEFINE_string("input_fname_pattern", "*.png", "Glob pattern of filename of input images [*]") flags.DEFINE_string("checkpoint_dir", "model", "Directory name to save the models [model]") flags.DEFINE_string("results_dir", "results", "Directory name to save the checkpoints [results]") flags.DEFINE_string("sample_dir", "samples", "Directory name to save the image samples [samples]") flags.DEFINE_boolean("is_crop", True, "True for training, False for testing [False]") flags.DEFINE_boolean("visualize", False, "True for visualizing, False for nothing [False]") flags.DEFINE_integer( "save_epoch", 10, "The size of the output images to produce. If None, same value as output_height [None]" ) flags.DEFINE_boolean( "use_batchsize_for_prediction", True, "Use batch size for prediction (True) or one sample only (False) [True]" ) flags = flags.FLAGS if flags.input_width is None: flags.input_width = flags.input_height if flags.output_width is None: flags.output_width = flags.output_height if not os.path.exists(flags.checkpoint_dir): os.makedirs(flags.checkpoint_dir) if not os.path.exists(flags.sample_dir): os.makedirs(flags.sample_dir) # initialize image subscribers and publisher rospy.init_node('underwater_camera_node', anonymous=True) image_sub = message_filters.Subscriber('/depth_camera/image_raw', Image) depth_sub = message_filters.Subscriber('/depth_camera/depth/image_raw', Image) camera_info_sub = message_filters.Subscriber('/depth_camera/camera_info', CameraInfo) underwater_image_pub = rospy.Publisher( '/underwater_camera_watergan/image_raw', Image, queue_size=10) underwater_camera_info_pub = rospy.Publisher( '/underwater_camera_watergan/camera_info', CameraInfo, queue_size=10) ts = message_filters.TimeSynchronizer( [image_sub, depth_sub, camera_info_sub], 10) ts.registerCallback(camera_callback)
rospy.loginfo("Great, You have {} CUDA device!".format(torch.cuda.device_count())) print ('Available devices ', torch.cuda.device_count()) print ('Current cuda device ', torch.cuda.current_device()) else: rospy.logerr("Sorry, You DO NOT have a CUDA device!") #Declare DDRNet model and load weights net = make_model('ddrnet', num_classes=12).cuda() load_checkpoint = torch.load(weight_file) state_dict = dict() for key in load_checkpoint['state_dict'].keys(): new_key = key.replace('module.','') state_dict[new_key] = load_checkpoint['state_dict'][key] net.load_state_dict(state_dict) net.eval() # Declare a TF transform broadcaster tf_broadcaster = tf.TransformBroadcaster() depth_sub = message_filters.Subscriber(depth_image_topic, Image) rgb_sub = message_filters.Subscriber(rgb_image_topic, Image) pointcloud_sub = message_filters.Subscriber(pointcloud_topic, PointCloud2) camera_pose_sub = message_filters.Subscriber(camera_pose_topic, PoseStamped) ts = message_filters.TimeSynchronizer([depth_sub, rgb_sub,pointcloud_sub, camera_pose_sub], 1) ts.registerCallback(callback) print('SSC READY!') rospy.spin()
4 def callback(image, camera_info): 5 # Solve all of perception here... 6 7 image_sub = message_filters.Subscriber('image', Image) 8 info_sub = message_filters.Subscriber('camera_info', CameraInfo) 9
current_pos = [int(currentX), int(currentY)] result = aStarAlog(map2d, current_pos, [goal_p.x, goal_p.y]) path = [] for item in result: # store the path as a list odom_scale_x = int(item.row * resolution + origin_X) odom_scale_y = int(item.col * resolution + origin_Y) path.append([odom_scale_x, odom_scale_y]) ask_waypoint_callback(None) if __name__ == "__main__": #global pub rospy.init_node("aStarPath") map_sub = message_filters.Subscriber('/map', OccupancyGrid) odom_sub = message_filters.Subscriber('/odom', Odometry) waypoint_bool_sub = rospy.Subscriber('/ask_waypoint', Bool, ask_waypoint_callback) waypoint_sub = rospy.Subscriber('/frontier_goal', Point, frontier_goal) ts = message_filters.TimeSynchronizer([map_sub, odom_sub], 10) pub = rospy.Publisher('/waypoints', Point) pub2 = rospy.Publisher('/frontier_goal_request', Bool) pub2.publish(True) ts.registerCallback(map_odom_callback) rospy.spin()
#out = model.encode(x) # calc = f_model.calc() get_f = theano.function([x], out) # get_nn = theano.function([x], calc) ann = libfann.neural_net() ann.create_from_file(networkPath) shape = (28, 28, 1) flat_shape = (1, 28 * 28) bridge = CvBridge() path = "/home/rik/nav-data/" rospy.init_node('live_error_test', anonymous=True) rospy.loginfo("Node initialized") odom_sub = message_filters.Subscriber('odom', Odometry) image_sub = message_filters.Subscriber('/sudo/bottom_webcam/image_raw', Image) ts = ApproximateTimeSynchronizer([odom_sub, image_sub], 100, 0.2) ts.registerCallback(live_test) rospy.loginfo("Callbacks registered") try: rospy.spin() except KeyboardInterrupt: print "Shutting down" cv2.destroyAllWindows()
def __init__(self, boards, service_check=True, synchronizer=message_filters.TimeSynchronizer, flags=0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags=0, max_chessboard_speed=-1, queue_size=1): if service_check: # assume any non-default service names have been set. Wait for the service to become ready for svcname in ["camera", "left_camera", "right_camera"]: remapped = rospy.remap_name(svcname) if remapped != svcname: fullservicename = "%s/set_camera_info" % remapped print("Waiting for service", fullservicename, "...") try: rospy.wait_for_service(fullservicename, 5) print("OK") except rospy.ROSException: print("Service not found") rospy.signal_shutdown('Quit') self._boards = boards self._calib_flags = flags self._checkerboard_flags = checkerboard_flags self._pattern = pattern self._camera_name = camera_name self._max_chessboard_speed = max_chessboard_speed lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) ts = synchronizer([lsub, rsub], 4) ts.registerCallback(self.queue_stereo) msub = message_filters.Subscriber('image', sensor_msgs.msg.Image) msub.registerCallback(self.queue_monocular) self.set_camera_info_service = rospy.ServiceProxy( "%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) self.set_left_camera_info_service = rospy.ServiceProxy( "%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) self.set_right_camera_info_service = rospy.ServiceProxy( "%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) self.q_mono = BufferQueue(queue_size) self.q_stereo = BufferQueue(queue_size) self.c = None self._last_display = None mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start()
def __init__(self): # initialize the node named image_processing rospy.init_node('image_processing', anonymous=True) # initialize a publisher to send images from camera1 to a topic named image_topic1 self.image_pub1 = rospy.Publisher("image_topic1", Image, queue_size=10) # initialize a subscriber to recieve messages rom a topic named /robot/camera1/image_raw and use callback function to recieve data self.image_sub1 = message_filters.Subscriber( "/camera1/robot/image_raw", Image) #receive image from second camera self.image_sub2 = message_filters.Subscriber( "/camera2/robot/image_raw", Image) # Synchronize subscriptions into one callback timesync = message_filters.TimeSynchronizer( [self.image_sub1, self.image_sub2], 10) timesync.registerCallback(self.callback1) # initialize the bridge between openCV and ROS self.bridge = CvBridge() # initialize a publisher to send joints' angular position to a topic called joints_pos self.joints_pub = rospy.Publisher("joints_pos", Float64MultiArray, queue_size=10) #initialize a publisher to send position of orage sphere to to a topic name target_pos self.target_pub = rospy.Publisher("target_pos", Float64MultiArray, queue_size=10) #Publisher of all joints self.joint1_pub = rospy.Publisher( "/robot/joint1_position_controller/command", Float64, queue_size=10) self.joint2_pub = rospy.Publisher( "/robot/joint2_position_controller/command", Float64, queue_size=10) self.joint3_pub = rospy.Publisher( "/robot/joint3_position_controller/command", Float64, queue_size=10) self.joint4_pub = rospy.Publisher( "/robot/joint4_position_controller/command", Float64, queue_size=10) #save current angles so we can use least squares self.current_angles = [0, 0, 0, 0] #hardcoded them so that i do not calculate ratio every time self.pixel2meter_image1 = 0.03955706129619173 self.pixel2meter_image2 = 0.0391203563104617 # record the begining time self.time_initial = rospy.get_time() self.theta3 = np.array([0]) self.previous_theta1 = 0 self.previous_theta2 = 0 #hardcode yellow and blue coordinates since it does not move and this improves accuracy self.detect_yellow_image1 = np.array([399, 532]) self.detect_yellow_image2 = np.array([399, 532]) self.detect_blue_image1 = np.array([399, 472]) self.detect_blue_image2 = np.array([399, 472]) # joint angles in last iteration self.q_prev_observed = np.array([0.0, 0.0, 0.0, 0.0]) self.prev_pos = np.array([0.0, 0.0, 9.0]) # initialize errors self.time_previous_step = np.array([rospy.get_time()], dtype='float64') # initialize error and derivative of error for trajectory tracking self.error = np.array([0.0, 0.0, 0.0], dtype='float64') self.error_d = np.array([0.0, 0.0, 0.0], dtype='float64') self.last_green_image1 = [0, 0] self.last_green_image2 = [0, 0] self.last_red_image1 = [0, 0] self.last_red_image2 = [0, 0]
def __init__(self, config): rospy.init_node("gqcnn_base_grasp", anonymous=True) # Moveit! setup moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.arm = moveit_commander.MoveGroupCommander('arm') self.gripper = moveit_commander.MoveGroupCommander('gripper') self.arm.set_start_state_to_current_state() self.arm.set_pose_reference_frame('base') self.arm.set_planner_id('SBLkConfigDefault') self.arm.set_planning_time(10) self.arm.set_max_velocity_scaling_factor(0.04) self.arm.set_max_acceleration_scaling_factor(0.04) self.arm.set_goal_orientation_tolerance(0.1) self.arm.set_workspace([-2, -2, -2, 2, 2, 2]) self.gripper.set_goal_joint_tolerance(0.2) rospy.loginfo(self.arm.get_pose_reference_frame()) #base rospy.loginfo(self.arm.get_planning_frame()) #world # messgae filter for image topic, need carefully set./camera/depth_registered/sw_registered/image_rect,,/camera/rgb/image_rect_color rospy.loginfo('wait_for_message') ############## rospy.wait_for_message("/camera/rgb/image_raw", Image) ########### rospy.wait_for_message("/camera/depth/image", Image) ########### rospy.loginfo('start_callback') ########### self.image_sub = message_filters.Subscriber("/camera/rgb/image_raw", Image) self.depth_sub = message_filters.Subscriber("/camera/depth/image", Image) self.camera_info_topic = "/camera/rgb/camera_info" self.camera_info = rospy.wait_for_message(self.camera_info_topic, CameraInfo) rospy.loginfo(self.camera_info.header.frame_id) ###### self.ts = message_filters.ApproximateTimeSynchronizer( [self.image_sub, self.depth_sub], 1, 1) self.ts.registerCallback(self.cb) self.color_msg = Image() self.depth_msg = Image() self.mask = Image() # bounding box for objects self.bounding_box = BoundingBox() self.bounding_box.maxX = 420 self.bounding_box.maxY = 250 self.bounding_box.minX = 90 self.bounding_box.minY = 40 self.bridge = CvBridge() # transform listener self.listener = tf.TransformListener() # compute_ik service self.ik_srv = rospy.ServiceProxy('/compute_ik', GetPositionIK) rospy.loginfo("Waiting for /compute_ik service...") self.ik_srv.wait_for_service() rospy.loginfo("Connected!") self.service_request = PositionIKRequest() self.service_request.group_name = 'arm' self.service_request.timeout = rospy.Duration(2) self.service_request.avoid_collisions = True # signal self.start = 0
def __init__(self, chess_size, dim, approximate=0): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim # make sure n_cols is not smaller than n_rows, otherwise error computation will be off if self.board.n_cols < self.board.n_rows: self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols image_topic = rospy.resolve_name("monocular") + "/image_rect" camera_topic = rospy.resolve_name("monocular") + "/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] if approximate <= 0: sync = message_filters.TimeSynchronizer else: sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate) tsm = sync([ message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono ], 10) tsm.registerCallback(self.queue_monocular) left_topic = rospy.resolve_name("stereo") + "/left/image_rect" left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" right_topic = rospy.resolve_name("stereo") + "/right/image_rect" right_camera_topic = rospy.resolve_name( "stereo") + "/right/camera_info" tosync_stereo = [(left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo)] tss = sync([ message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo ], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue() self.q_stereo = Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) self.sc = StereoCalibrator([self.board])
# Close matcher after 5 seconds time.sleep(5) proc.send_signal(signal.SIGINT) rospy.loginfo("GPU matcher closed...") # Start again macther after 5 seconds of being closed #time.sleep(5) proc = subprocess.Popen(['rosrun', 'ug_stereomatcher', 'UG_matcher_gpu']) rospy.loginfo("GPU matcher openned...") # ********* MAIN ********* if __name__ == '__main__': global proc rospy.init_node('RHmatcher_hack', anonymous=True) ### subscribers subDV = message_filters.Subscriber('output_disparityV', DisparityImage) subDH = message_filters.Subscriber('output_disparityH', DisparityImage) ts = message_filters.TimeSynchronizer([subDH, subDV], 1) ts.registerCallback(messagesCB) subDVf = message_filters.Subscriber('output_stackV', foveatedstack) subDHf = message_filters.Subscriber('output_stackH', foveatedstack) tsf = message_filters.TimeSynchronizer([subDHf, subDVf], 1) tsf.registerCallback(messagesCBF) proc = subprocess.Popen(['rosrun', 'ug_stereomatcher', 'UG_matcher_gpu']) rospy.loginfo("matcher.py node ready!") rospy.spin()
picto.action = Pictogram.ADD picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8) rospy.loginfo("%s early taking off %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time)) else: continue picto.header.frame_id = ref_st.header.frame_id picto.header.stamp = ref_st.header.stamp arr.pictograms.append(picto) if len(arr.pictograms) > 0: pub.publish(arr) ref_contact_states_queue.pop(0) act_contact_states_queue.pop(0) ref_contact_states_queue.append(ref) act_contact_states_queue.append(act) if __name__ == "__main__": rospy.init_node("landing_time_detector") pub = rospy.Publisher("~pictogram_array", PictogramArray) ref_contact_states_sub = message_filters.Subscriber('~input_ref', ContactStatesStamped) act_contact_states_sub = message_filters.Subscriber('~input_act', ContactStatesStamped) buffer_size = 5 ref_contact_states_queue = [] act_contact_states_queue = [] ts = message_filters.TimeSynchronizer([ref_contact_states_sub, act_contact_states_sub], 10) ts.registerCallback(callback) rospy.spin()
pose = KpsToGrasppose(ret, img, depth_raw, M_CL, M_BL, cameraMatrix) msg2 = Float64MultiArray() msg2.data = pose pub_res.publish(msg2) except CvBridgeError as e: print(e) if __name__ == '__main__': # initialize ros node rospy.init_node("Bin_picking") # Bridge to convert ROS Image type to OpenCV Image type cv_bridge = CvBridge() cv2.WITH_QT = False # Get camera calibration parameters cam_param = rospy.wait_for_message('/camera/rgb/camera_info', CameraInfo, timeout=None) # Subscribe to rgb and depth channel image_sub = message_filters.Subscriber("/camera/rgb/image_rect_color", Image) depth_sub = message_filters.Subscriber("/camera/depth_registered/image", Image) ts = message_filters.ApproximateTimeSynchronizer([image_sub, depth_sub], 1, 0.1) ts.registerCallback(kinect_rgbd_callback) rospy.spin()
def __init__(self, camera_name, rgb_topic, depth_topic, camera_info_topic, choice=None): """ @brief A class to obtain time synchronized RGB and Depth frames from a camera topic and find the 3D position of the point wrt the required frame of reference. @param camera_name Just a relevant name for the camera being used. @param rgb_topic The topic that provides the rgb image information. @param depth_topic The topic that provides the depth image information. @param camera_info_topic The topic that provides the camera information. @param choice If the camera used is a real or simulated camera based on commandline arg. """ self.camera_name = camera_name self.rgb_topic = rgb_topic self.depth_topic = depth_topic self.camera_info_topic = camera_info_topic self.choice = choice self.poses = [] self.rays = [] self.OBlobs_x = [] self.OBlobs_y = [] self.OBlobs_z = [] self.colors = [] self.is_updated = False self.found_objects = False self.latest_rgb = None self.latest_depth_32FC1 = None tfBuffer = tf2_ros.Buffer() self.br = tf2_ros.TransformBroadcaster() self.lis = tf2_ros.TransformListener(tfBuffer) self.bridge = CvBridge() self.camera_model = image_geometry.PinholeCameraModel() # self.marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) # cv2.namedWindow("Image window", cv2.WINDOW_NORMAL) # cv2.setMouseCallback("Image window", self.mouse_callback) # self.br = tf.TransformBroadcaster() # self.lis = tf.TransformListener() # # Have we processed the camera_info and image yet? # self.ready_ = False q = 1 self.pose3D_pub = rospy.Publisher('object_location', OBlobs, queue_size=q) self.sub_rgb = message_filters.Subscriber(rgb_topic, Image, queue_size=q) self.sub_depth = message_filters.Subscriber(depth_topic, Image, queue_size=q) self.sub_camera_info = message_filters.Subscriber(camera_info_topic, CameraInfo, queue_size=q) # self.tss = message_filters.ApproximateTimeSynchronizer([self.sub_rgb, self.sub_depth, self.sub_camera_info], queue_size=15, slop=0.4) self.tss = message_filters.ApproximateTimeSynchronizer( [self.sub_rgb, self.sub_depth, self.sub_camera_info], queue_size=q, slop=0.5) #self.tss = message_filters.TimeSynchronizer([sub_rgb], 10) self.tss.registerCallback(self.callback) rospy.loginfo('Camera {} initialised, {}, {}, {}'.format( self.camera_name, rgb_topic, depth_topic, camera_info_topic))
def __init__(self): self.image_topic = rospy.get_param('~image_topic') self.fusion_topic = "/detection/fusion_tools/objects" # rospy.get_param('~fusion_topic') self.lanefusion_t = "/lane_fusion" self.output_image = rospy.get_param('~output_image') self.output_lane = rospy.get_param('~output_lane') self.weight_path = rospy.get_param('~weight_path') self.use_gpu = rospy.get_param('~use_gpu') self.init_lanenet() self.bridge = CvBridge() ## None sync (2 separate msg handlers) ## sub_image = rospy.Subscriber(self.image_topic, Image, self.img_callback, queue_size=1) ## sub_fused = rospy.Subscriber(self.fusion_topic, DetectedObjectArray, self.objs_callback, queue_size=1) image_sub = message_filters.Subscriber(self.image_topic, Image) fused_sub = message_filters.Subscriber(self.fusion_topic, DetectedObjectArray) tsync_sub = message_filters.ApproximateTimeSynchronizer( [image_sub, fused_sub], 32, 0.1, allow_headerless=True) tsync_sub.registerCallback(self.sync_cb) self.oboi = ['person', 'bicycle', 'car', 'motorbike', 'bus', 'truck'] # 'traffic light', 'stop sign' later # how to sync up ingest speed with prior image, use simple solution for now: # - At entry of img_callback, set lock / flag so subsequent DetectedObjectArray won't enqueue # - At exit of img_callback, release the flag # - At entry of objs_callback, simply return when sync queue is locked # filtered objs self.objs = [] self.sync = False self.pub_image = rospy.Publisher(self.output_image, Image, queue_size=4) self.pub_lfuse = rospy.Publisher(self.lanefusion_t, LaneObstacle, queue_size=4) self.lane_obst = LaneObstacle() self.lane_obst.radius = 5000.0 # (m) self.lane_obst.centerdev = 0.0 # (cm) self.lane_obst.anglerror = 0.0 # (o) self.lane_obst.dist2obst_e = -1.0 # (m) self.lane_obst.dist2obst_l = -1.0 # (m) self.lane_obst.dist2obst_r = -1.0 # (m) self.h = 600 # pix self.w = 800 # pix self.ym_per_pix = 20. / 600 # m/pix self.xm_per_pix = 3.6 / 540 # m/pix self.M = np.array([[-9.75609756e-01, -1.31707317e+00, 7.90243902e+02], [-2.84217094e-16, -2.92682927e+00, 1.17073171e+03], [-5.26327952e-19, -3.29268293e-03, 1.00000000e+00]]) self.Minv = np.array( [[3.25000000e-01, -4.50000000e-01, 2.70000000e+02], [0.00000000e+00, -3.41666667e-01, 4.00000000e+02], [0.00000000e+00, -1.12500000e-03, 1.00000000e+00]]) self.lanes_image = None self.llane_image = None self.rlane_image = None # L/R lane line instances self.lline = Laneline() self.rline = Laneline() self.q_RDH = dq([], maxlen=10) # Set initial ^2 fitting parameters, for ego lane's center self.q_fit = dq([], maxlen=5) # vertical at pixel 400 self.q_fit.append(np.array([0.0, 0.0, 400.])) self.count = 0
def __init__(self): # Sub topics self.topic_rgb_image = '/zed/rgb/image_rect_color' self.topic_depth_image = '/zed/depth/depth_registered' # Image: 32-bit depth values in meters # Pub topics self.topic_yolo_output_img = 'yolo/labelled_img' self.topic_tracker_output_img = 'labelled_img' self.new_img_received = False self.rgb_img = None self.rgb_img_output = None self.depth_img = None self.bridge = CvBridge() # Vars Tracker self.selected_target_bbox = None # Stores the bbox of the selected target, shortlisted from yolo_bbox_arr self.yolo_bbox_arr = [] # Stores array of bboxes returned by object detection code self.target_class = 'person' # The class to be tracked. self.tracker_min_iou = 0.3 # Min value of IoU for a bbox to be considered same as tracked object. self.tracker_init_min_iou = 0.2 # This is used for first initialization of tracker only self.threshold_confidence = 0.4 # prob of class must be greater than this threshold for us to use it. self.tracker = None # Object to store tracker: cv2.TrackerKCF_create() or cv2.TrackerMOSSE_create() self.fps = FPS().start() # Load yolo config rospack = rospkg.RosPack() path = rospack.get_path('sdroller_track_detect') object_detection_config = rospy.get_param("/object_detection_config") self.config = yaml.load(open(object_detection_config)) self.yolo = YOLO(path + self.config['classification']['model'], path + self.config['classification']['anchors'], path + self.config['classification']['classes']) # Subscribers '''We increase the default buff_size because it is smaller than the camera's images, and so if the node doesn't processing them fast enough, there will be a number of images backed up in some queue and it appears as a lag in the video stream. We also use the TimeSynchronizer to subscribe to multiple topics. ''' buff_size_bytes = 8 * 1024 * 1024 # 8MB self.rgb_img_sub = message_filters.Subscriber( self.topic_rgb_image, Image, queue_size=1, buff_size=buff_size_bytes) self.depth_img_sub = message_filters.Subscriber( self.topic_depth_image, Image, queue_size=1, buff_size=buff_size_bytes) self.ts = message_filters.ApproximateTimeSynchronizer( [self.rgb_img_sub, self.depth_img_sub], 1, 0.1, allow_headerless=False) self.ts.registerCallback(self.callback_rgb_depth_imgs) # Publish the image with detection boxes drawn self.yolo_labelled_img_pub = rospy.Publisher(self.topic_yolo_output_img, Image, queue_size=1) self.tracker_labelled_img_pub = rospy.Publisher(self.topic_tracker_output_img, Image, queue_size=1) # Get image details rgb_img_sample = rospy.wait_for_message(self.topic_rgb_image, Image) self.img_height = rgb_img_sample.height self.img_width = rgb_img_sample.width # Set the value of tracker_bbox to middle of the image, where a person normally detected standing in front of robot self.tracker_bbox = bbox() self.tracker_bbox.xmin = int(self.img_width * (1.0 / 3)) self.tracker_bbox.ymin = int(self.img_height * (1.0 / 4)) self.tracker_bbox.xmax = int(self.img_width * (2.0 / 3)) self.tracker_bbox.ymax = int(self.img_height)
def __init__(self): self.SIZE = 1280 * 960 * 3 # Just to set the buffer_size self.THRESH = CLASS_THRESHOLDS self.MAP_SIDE = 1000 self.SCALE = 20 publish_rois_debug = rospy.get_param('~publish_rois_debug', False) self.draw_arrows = rospy.get_param('~draw_arrows', True) self.y_offset = rospy.get_param('~y_offset', 0) self.roi_height = rospy.get_param('~height', 768) self.pub_demo = rospy.Publisher('nice_demo', Image, queue_size=10) self.pub_complex_demo = rospy.Publisher('very_nice_demo', Image, queue_size=10) self.pub_map = rospy.Publisher('map', Image, queue_size=10) if publish_rois_debug: self.pub_rois = rospy.Publisher('rois_debug', Image, queue_size=10) self.bridge = CvBridge() map_sync = message_filters.TimeSynchronizer([ message_filters.Subscriber( "topdown_view", Image, buff_size=self.SIZE * 2), message_filters.Subscriber( "obstacles", numpy_msg(ObstacleList), buff_size=self.SIZE * 2) ], 10) bboxes_sync = message_filters.TimeSynchronizer([ message_filters.Subscriber("/stereo_camera/left/image_rect_color", Image, buff_size=self.SIZE * 2), message_filters.Subscriber("filtered_detection", numpy_msg(ObstacleList), buff_size=self.SIZE * 2) ], 10) freemap_sync = message_filters.TimeSynchronizer([ message_filters.Subscriber("/stereo_camera/left/image_rect_color", Image, buff_size=self.SIZE * 2), message_filters.Subscriber("filtered_detection", numpy_msg(ObjectList), buff_size=self.SIZE * 2), message_filters.Subscriber( "free_mask", Image, buff_size=self.SIZE * 2) ], 10) map_sync.registerCallback(self.map_callback) bboxes_sync.registerCallback(self.bboxes_callback) freemap_sync.registerCallback(self.freemap_callback) if publish_rois_debug: externalrois_sync = message_filters.TimeSynchronizer([ message_filters.Subscriber( "/stereo_camera/left/image_rect_color", Image, buff_size=self.SIZE * 2), message_filters.Subscriber("filtered_detection", numpy_msg(ObjectList), buff_size=self.SIZE * 2), message_filters.Subscriber("/stereo_camera/obstacles", numpy_msg(ObstacleList), buff_size=self.SIZE * 2), message_filters.Subscriber("/stereo_camera/freeobs_mask", Image, buff_size=self.SIZE * 2) ], 10) externalrois_sync.registerCallback(self.externalrois_callback)
def __init__(self,opt,save_img=False): source, weights, self.view_img, self.save_txt, imgsz = opt.source, opt.weights, opt.view_img, opt.save_txt, opt.img_size self.webcam = source.isnumeric() or source.endswith('.txt') or source.lower().startswith( ('rtsp://', 'rtmp://', 'http://')) self.save_img = save_img # Directories # self.save_dir = Path(increment_path(Path(opt.project) / opt.name, exist_ok=opt.exist_ok)) # increment run # (self.save_dir / 'labels' if self.save_txt else self.save_dir).mkdir(parents=True, exist_ok=True) # make dir # Initialize set_logging() self.device = select_device(opt.device) self.half = self.device.type != 'cpu' # half precision only supported on CUDA # Load model self.model = attempt_load(weights, map_location=self.device) # load FP32 model imgsz = check_img_size(imgsz, s=self.model.stride.max()) # check img_size if self.half: self.model.half() # to FP16 #"Fusing layers..." # Second-stage classifier self.classify = False if self.classify: modelc = load_classifier(name='resnet101', n=2) # initialize modelc.load_state_dict(torch.load('weights/resnet101.pt', map_location=self.device)['model']).to(self.device).eval() # Get names and colors self.names = self.model.module.names if hasattr(self.model, 'module') else self.model.names self.colors = [[random.randint(0, 255) for _ in range(3)] for _ in self.names] # Run inference t0 = time.time() img = torch.zeros((1, 3, imgsz, imgsz), device=self.device) # init img _ = self.model(img.half() if self.half else img) if self.device.type != 'cpu' else None # run once # ROS self.bridge = CvBridge() self.pub = rospy.Publisher('/gesture_detection_image', Image,queue_size=10) # rgb_image_sub = rospy.Subscriber("/spencer/sensors/rgbd_front_top/color/image_raw", Image, self.image_callback) rgb_image_sub = message_filters.Subscriber("/spencer/sensors/rgbd_front_top/color/image_raw", Image) # depth_image_sub = message_filters.Subscriber('/spencer/sensors/rgbd_front_top/depth/image_rect_raw', Image) depth_image_sub = message_filters.Subscriber('/spencer/sensors/rgbd_front_top/aligned_depth_to_color/image_raw', Image) cmd_vel_sub = message_filters.Subscriber('/tb_control/wheel_odom', Odometry) self.time_sync = message_filters.ApproximateTimeSynchronizer([rgb_image_sub, depth_image_sub,cmd_vel_sub], 10, 1) # self.time_sync = message_filters.ApproximateTimeSynchronizer([rgb_image_sub, depth_image_sub], 5, 0.2) self.time_sync.registerCallback(self.image_callback) self.gesture_pub = rospy.Publisher('/gesture_recognition_result', Header, queue_size=10) self.distance_threshold = 450 self.conf_threshold = 0.7 self.last_gesture = None self.last_detect_time = 0 self.timeout = rospy.Duration(3)
throttle_np = np.zeros((1, )) throttle_np[0] = throttle.pedal_cmd throttle_list.append(throttle_np) steering_np = np.zeros((1, )) steering_np[0] = steering.steering_wheel_angle_cmd steering_list.append(steering_np) brake_np = np.zeros((1, )) brake_np[0] = brake.pedal_cmd brake_list.append(brake_np) if __name__ == '__main__': img_sub = message_filters.Subscriber( '/vehicle/front_camera/image_rect_color', Image) pc2_sub = message_filters.Subscriber('/vehicle/velodyne_points', PointCloud2) imu_sub = message_filters.Subscriber('/vehicle/imu/data_raw', Imu) cmd_vel_sub = message_filters.Subscriber('/vehicle/cmd_vel', Twist) throttle_sub = message_filters.Subscriber('/vehicle/throttle_cmd', ThrottleCmd) steering_sub = message_filters.Subscriber('/vehicle/steering_cmd', SteeringCmd) brake_sub = message_filters.Subscriber('/vehicle/brake_cmd', BrakeCmd) ts = message_filters.ApproximateTimeSynchronizer([ img_sub, pc2_sub, imu_sub, cmd_vel_sub, throttle_sub, steering_sub, brake_sub ], 10,
cv_img_wv5 = cv.cvtColor(cv_img_wv5, cv.COLOR_BGR2GRAY) #Fusion cv_img_merged = np.zeros(cv_img_wv1.shape) cv.accumulate(cv_img_wv1, cv_img_merged); cv.accumulate(cv_img_wv2, cv_img_merged); cv.accumulate(cv_img_wv3, cv_img_merged); cv.accumulate(cv_img_wv4, cv_img_merged); cv.accumulate(cv_img_wv5, cv_img_merged); cv_img_merged = cv_img_merged/5 cv_img_merged = cv_img_merged.astype(np.uint8) #Publish message = bridge.cv2_to_imgmsg(cv_img_merged, encoding="mono8") message.header.stamp = stamp pub.publish(message) image_sub_wv1 = message_filters.Subscriber('/camera/image_wavelength_0', Image) #644 image_sub_wv2 = message_filters.Subscriber('/camera/image_wavelength_1', Image) #653 image_sub_wv3 = message_filters.Subscriber('/camera/image_wavelength_9', Image) #661 image_sub_wv4 = message_filters.Subscriber('/camera/image_wavelength_10', Image) #607 image_sub_wv5 = message_filters.Subscriber('/camera/image_wavelength_20', Image) #701 print('initialized') rospy.init_node('HyperspectralFusion') pub = rospy.Publisher('/camera/image_hyperspec', Image, queue_size=10) ts = message_filters.ApproximateTimeSynchronizer([image_sub_wv1, image_sub_wv2, image_sub_wv3, image_sub_wv4, image_sub_wv5], 10, 0.1, allow_headerless=False) ts.registerCallback(callback) rospy.spin()
def __init__(self, ctx, output_world, mesh_dir, reference_frame): self.human_cameras_ids = {} self.ctx = ctx self.human_bodies = {} self.target = ctx.worlds[output_world] self.target_world_name = output_world self.reference_frame = reference_frame self.mesh_dir = mesh_dir self.human_meshes = {} self.human_aabb = {} self.nb_gaze_detected = {} self.human_perception_to_uwds_ids = {} self.detection_time = None self.reco_durations = [] self.record_time = False self.robot_name = rospy.get_param("robot_name", "pepper") self.is_robot_moving = rospy.get_param("is_robot_moving", False) self.ros_pub = {"voice": rospy.Publisher("multimodal_human_monitor/voice_attention_point", PointStamped, queue_size=5), "gaze": rospy.Publisher("multimodal_human_monitor/gaze_attention_point", PointStamped, queue_size=5), "reactive": rospy.Publisher("multimodal_human_monitor/monitoring_attention_point", PointStamped, queue_size=5), "monitoring_attention_point": rospy.Publisher("head_manager/head_monitoring_target", TargetWithPriority, queue_size=5), "tf": rospy.Publisher("/tf", TFMessage, queue_size=10)} self.log_pub = {"isLookingAt": rospy.Publisher("predicates_log/lookingat", String, queue_size=5), "isPerceiving": rospy.Publisher("predicates_log/perceiving", String, queue_size=5), "isSpeaking": rospy.Publisher("predicates_log/speak", String, queue_size=5), "isSpeakingTo": rospy.Publisher("predicates_log/speakingto", String, queue_size=5), "isNear": rospy.Publisher("predicates_log/near", String, queue_size=5), "isClose": rospy.Publisher("predicates_log/close", String, queue_size=5), "isMonitoring": rospy.Publisher("predicates_log/monitoring", String, queue_size=5)} self.ros_sub = {"gaze": message_filters.Subscriber("wp2/gaze", GazeInfoArray), "voice": message_filters.Subscriber("wp2/voice", VoiceActivityArray), "person": message_filters.Subscriber("wp2/track", TrackedPersonArray)} self.ros_services = {"monitor_humans": rospy.Service("multimodal_human_monitor/monitor_humans", MonitorHumans, self.handle_monitor_humans), "find_alternate_id": rospy.Service("multimodal_human_monitor/find_alternate_id", FindAlternateId, self.handle_find_alternate_id), "global_monitoring": rospy.Service("multimodal_human_monitor/global_monitoring", SetBool, self.handle_global_monitoring)} self.ts = message_filters.TimeSynchronizer([self.ros_sub["gaze"], self.ros_sub["voice"], self.ros_sub["person"]], 50) self.ts.registerCallback(self.callback) self.head_signal_dq = deque() self.already_removed_nodes = [] self.current_situations_map = {} self.humans_to_monitor = [] self.reco_id_table = {} self.inv_reco_id_table = {} self.human_perceived = [] self.previous_human_perceived = [] self.human_speaking = [] self.previous_human_speaking = [] self.human_speakingto = {} self.previous_human_speakingto = {} self.human_lookat = {} self.previous_human_lookat = {} self.human_distances = {} self.human_close = [] self.previous_human_close = [] self.human_near = [] self.previous_human_near = [] self.is_active = True self.tf_buffer = tf2_ros.Buffer(rospy.Duration(TF_CACHE_TIME), debug=False) self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) try: nodes_loaded = ModelLoader().load(self.mesh_dir + "face.blend", self.ctx, world=output_world, root=None, only_meshes=True, scale=1.0) for n in nodes_loaded: if n.type == MESH: self.human_meshes["face"] = n.properties["mesh_ids"] self.human_aabb["face"] = n.properties["aabb"] except Exception as e: rospy.logwarn("[multimodal_human_provider] Exception occurred with %s : %s" % (self.mesh_dir + "face.blend", str(e)))
# Point(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), # Vector3(*vel / 5), Vector3(0, 0, 0), # Vector3(*acc / 5), Vector3(0, 0, 0), yaw, # 0, # pos_cmd[10] / 5, 0, [0, 0, 0], [0, 0, 0], id, PositionCommand.TRAJECTORY_STATUS_READY) pos_cmd_publisher.publish(pos_cmd) rospy.loginfo("[POS CMD: pos_cmd\ {} n".format(pos_cmd)) id += 1 rospy.init_node('playback_node') rospy.loginfo("started node\n") image_sub = message_filters.Subscriber('/pcl_render_node/depth', SensorImage) # pos_cmd_sub = message_filters.Subscribe('/planning/pos_cmd', PositionCommand) odom_sub = message_filters.Subscriber('/visual_slam/odom', Odometry) # rospy.Subscriber("/visual_slam/odom", Odometry, callback, buff_size=10) rospy.Subscriber("/move_base_simple/goal", PoseStamped, goalCallback, buff_size=10) # ts = message_filters.ApproximateTimeSynchronizer([image_sub, odom_sub], 5, 0.1, allow_headerless=True) ts = message_filters.ApproximateTimeSynchronizer([image_sub, odom_sub], 100, 1) ts.registerCallback(callback) rospy.spin()
def __init__(self): """ Constructor. """ # Detection target ID (person) self.target = 15 # visitor detected self.visitor = 0 self.confirm_attempts = 0 # Minimum confidence for acceptable detections self.confidence = 0.5 # Converted depth_image self.depth_image = None # Publishing rate self.rate = rospy.Rate(10) # Number of detections self.number_of_detections = 0 # Detection messages self.detections = Detections() # Constant path self.path = str( Path(os.path.dirname(os.path.abspath(__file__))).parents[0]) # Define detection's target/s self.targets = [ "background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor" ] # Bounding boxes self.colours self.colours = np.random.uniform(0, 255, size=(len(self.targets), 3)) # Load the neural network serialised model self.net = cv2.dnn.readNetFromCaffe( self.path + "/data/nn_params/MobileNetSSD_deploy.prototxt.txt", self.path + "/data/nn_params/MobileNetSSD_deploy.caffemodel") # Distance (human-robot distance) and detection publishers self.detection_pub = rospy.Publisher('detections', Detections, queue_size=1) print("[INFO] Successful DNN Initialisation") # Subscriptions (via Subscriber package) rgb_sub = message_filters.Subscriber( "/xtion/rgb/image_rect_color", Image, queue_size=None) ## sam added queue size = None ## # depth_sub = message_filters.Subscriber("/xtion/depth_registered/hw_registered/image_rect_raw", Image) depth_sub = message_filters.Subscriber( "/xtion/depth_registered/image_raw", Image, queue_size=None) ## sam added queue size = None ## # Synchronize subscriptions ats = message_filters.ApproximateTimeSynchronizer( [rgb_sub, depth_sub], queue_size=1, slop=0.1) ## sam changed queue size from 5 and slop from 0.5 ats.registerCallback(self.processSubscriptions)
if (name == target_model_name): break idx += 1 # Get pose: current_pose = all_states.pose[idx] x = goal.position.x y = goal.position.y z = goal.position.z rx = goal.orientation.x ry = goal.orientation.y rz = goal.orientation.z rw = goal.orientation.w rospy.init_node("p2p_move", anonymous=True) state_sub = message_filters.Subscriber("/gazebo/model_states", ModelStates) goal_sub = message_filters.Subscriber("/get_goal", Pose) # goal_sub = message_filters.Subscriber("husky/get_goal", Pose) ts = message_filters.ApproximateTimeSynchronizer([state_sub, goal_sub], 10, 0.1, allow_headerless=True) ts.registerCallback(callback) # rospy.Subscriber("/gazebo/model_states", ModelStates, callback) pub = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size=10) target_pose = Pose() target_state = ModelState() velocity = 0.1 angular_velocity = 0.2 rospy.loginfo("Ready") rate = rospy.Rate(100)
json.dump(mpu9150_6_twist, open('./data/mpu9150_6_twist.txt','w')) json.dump(mpu9150_7_twist, open('./data/mpu9150_7_twist.txt','w')) json.dump(mpu9150_8_twist, open('./data/mpu9150_8_twist.txt','w')) json.dump(mpu9150_9_twist, open('./data/mpu9150_9_twist.txt','w')) json.dump(mpu9150_10_twist, open('./data/mpu9150_10_twist.txt','w')) json.dump(mpu9150_11_twist, open('./data/mpu9150_11_twist.txt','w')) json.dump(mpu9150_12_twist, open('./data/mpu9150_12_twist.txt','w')) json.dump(mpu9150_13_twist, open('./data/mpu9150_13_twist.txt','w')) json.dump(mpu9150_14_twist, open('./data/mpu9150_14_twist.txt','w')) json.dump(mpu9150_15_twist, open('./data/mpu9150_15_twist.txt','w')) counter = counter -1 rospy.init_node('mpu9150') mpu9150_0_sub = message_filters.Subscriber('mpu9150_0', Imu) mpu9150_1_sub = message_filters.Subscriber('mpu9150_1', Imu) mpu9150_2_sub = message_filters.Subscriber('mpu9150_2', Imu) mpu9150_3_sub = message_filters.Subscriber('mpu9150_3', Imu) mpu9150_4_sub = message_filters.Subscriber('mpu9150_4', Imu) mpu9150_5_sub = message_filters.Subscriber('mpu9150_5', Imu) mpu9150_6_sub = message_filters.Subscriber('mpu9150_6', Imu) mpu9150_7_sub = message_filters.Subscriber('mpu9150_7', Imu) mpu9150_8_sub = message_filters.Subscriber('mpu9150_8', Imu) mpu9150_9_sub = message_filters.Subscriber('mpu9150_9', Imu) mpu9150_10_sub = message_filters.Subscriber('mpu9150_10', Imu) mpu9150_11_sub = message_filters.Subscriber('mpu9150_11', Imu) mpu9150_12_sub = message_filters.Subscriber('mpu9150_12', Imu) mpu9150_13_sub = message_filters.Subscriber('mpu9150_13', Imu) mpu9150_14_sub = message_filters.Subscriber('mpu9150_14', Imu) mpu9150_15_sub = message_filters.Subscriber('mpu9150_15', Imu)
def __init__(self): rospy.init_node('mapping_node') # Mapping Constants self.origin = Pose() self.origin.position.x = 0. self.origin.position.y = 0. self.origin.orientation.w = 1. self.map_info = MapMetaData() self.map_info.map_load_time = rospy.Time.now() self.map_info.resolution = .05 # This matches the resolution of gmapping self.map_info.height = 746 self.map_info.width = 775 self.map_info.origin = self.origin # Sensor Model - certainty. When using no noise in lidar, keep these high #self.alpha = 0.9 #self.beta = 0.7 self.alpha = .999 self.beta = .999 # To stabilize our graph self.start_wait = rospy.get_time() self.twist = Twist() # Transform(s) # sensor to robot self.T_sensor = np.array([[np.cos(0), -1 * np.sin(0), 0.0], [np.sin(0), np.cos(0), 0.0], [0.0, 0.0, 1.0]]) # robot to world self.T_robot = np.array([[np.cos(0), -1 * np.sin(0), 0.0], [np.sin(0), np.cos(0), 0.0], [0.0, 0.0, 1]]) # occ_grid is a numpy matrix of probabilibites between 0 and 1 # Prefill with 0.5 for unknown occupancy self.occ_grid = np.repeat(np.matrix(np.repeat(.5, self.map_info.width)), self.map_info.height, axis=0) # ros_occ_grid is a ROS OccupancyGrid instance self.ros_occ_grid = OccupancyGrid() self.ros_occ_grid.info = self.map_info # Publishers self.map_pub = rospy.Publisher('/my_static_map', OccupancyGrid, queue_size=1) # Subscribers # pose_sub and scan_sub are synchronized. # when the updateMap callback is executed it receives a message from both. self.pose_sub = message_filters.Subscriber('/my_kalman_filter', PoseStamped) self.scan_sub = message_filters.Subscriber('/robot0/scan', LaserScan) self.ts = message_filters.ApproximateTimeSynchronizer( [self.pose_sub, self.scan_sub], 10, 0.1) self.ts.registerCallback(self.updateMap) # Subscribe to twist because we don't want to map when we are turning self.twist_sub = rospy.Subscriber('robot0/input_vel', Twist, self.updateTwist) rospy.spin()
def __init__(self): """Create subscribers and publishers.""" # Get parameters and initialize class variables. self.num_agents = rospy.get_param('/num_of_robots') robot_name = rospy.get_param('~robot_name') using_sim_kalman = False if rospy.get_param('/run_type') == 'sim': using_sim_kalman = rospy.get_param('/use_kalman') if not using_sim_kalman and rospy.get_param( '/ctrl_loop_freq') != rospy.get_param('/data_stream_freq'): rospy.logwarn( 'When not using Kalman filter in simulation, ctrl_loop_freq and ' 'data_stream_freq must be the same! Change these parameters in ' '`setup_sim.launch` and interval_sim in `definitions.inc`!' ) # Create publishers for commands pub_keys = [ robot_name + '_{}'.format(i) for i in range(self.num_agents) ] # Publisher for locations of nearest agents self.nearest = dict.fromkeys(pub_keys) for key in self.nearest.keys(): self.nearest[key] = rospy.Publisher('/' + key + '/nearest', OdometryArray, queue_size=1) # Publisher for locations of walls and obstacles self.avoid = dict.fromkeys(pub_keys) for key in self.avoid.keys(): self.avoid[key] = rospy.Publisher('/' + key + '/avoid', PoseArray, queue_size=1) # Create subscribers rospy.Subscriber('/map', OccupancyGrid, self.map_callback, queue_size=1) rospy.sleep(0.5) # Wait for first map_callback to finish rospy.Subscriber('/dyn_reconf/parameter_updates', Config, self.param_callback, queue_size=1) self.param_callback(None) if using_sim_kalman: topic_name = '/' + robot_name + '_{}/odom_est' else: topic_name = '/' + robot_name + '_{}/odom' subs = [ mf.Subscriber(topic_name.format(i), Odometry) for i in range(self.num_agents) ] self.ts = mf.ApproximateTimeSynchronizer( subs, 10, 0.11) # TODO: set this to be parametric as well self.ts.registerCallback(self.robot_callback) # Keep program from exiting rospy.spin()
print("...ok") runCommandClient = rospy.ServiceProxy('run_command', RunCommand) runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty) initCode = open( "SotInitSim.py", "r").read().split("\n") print("Stack of Tasks launched") launchScript(initCode,'initialize SoT') # Check table distance rospy.init_node('TablePrehension', anonymous=True) global subRH subRH = message_filters.Subscriber("cmd_right_hand_sim", PointStamped) global subLH subLH = message_filters.Subscriber("cmd_left_hand_sim", PointStamped) ts = message_filters.TimeSynchronizer([subRH,subLH], 1) ts.registerCallback(callbackCheck) # If no issue let's start raw_input() runCommandStartDynamicGraph() print("...done") time.sleep(2)
def __init__(self): self.node_name = "Obstacle Detecion Node" robot_name = rospy.get_param("~veh", "") # robot_name = "dori" self.show_marker = (rospy.get_param("~show_marker", "")) # self.show_marker=True self.show_image = (rospy.get_param("~show_image", "")) # bounding box parameters in mm self.show_bb = (rospy.get_param("~show_bb", "")) self.bb_len = (rospy.get_param("~bb_len", "")) self.bb_wid = (rospy.get_param("~bb_wid", "")) print 'Here comes bb_len' print self.bb_len # Load camera calibration parameters self.intrinsics = load_camera_intrinsics(robot_name) self.visualizer = Visualizer(robot_name=robot_name) # Create Publishers if (self.show_marker): self.pub_topic_marker = '/{}/obst_detect_visual/visualize_obstacles'.format( robot_name) self.publisher_marker = rospy.Publisher(self.pub_topic_marker, MarkerArray, queue_size=1) print "YEAH I GIVE YOU THE MARKER" if (self.show_image): self.pub_topic_img = '/{}/obst_detect_visual/image/compressed'.format( robot_name) self.publisher_img = rospy.Publisher(self.pub_topic_img, CompressedImage, queue_size=1) print "YEAH I GIVE YOU THE IMAGE" # if (self.show_bb): self.pub_topic_bb_linelist = '/{}/obst_detect_visual/bb_linelist'.format( robot_name) self.publisher_bblinelist = rospy.Publisher(self.pub_topic_bb_linelist, Marker, queue_size=1) print "YEAH I GIVE YOU THE BOUNDINGBOXMARKERLIST" self.bbmarker = Marker() self.bbmarker.header.frame_id = '{}'.format(robot_name) # self.bbmarker.ns = "points_and_lines" # self.bbmarker.id = 0; self.bbmarker.action = Marker.ADD self.bbmarker.type = Marker.LINE_STRIP self.bbmarker.lifetime = rospy.Time(10.0) self.bbmarker.pose.orientation.w = 1.0 self.bbmarker.scale.x = 0.02 self.bbmarker.color.b = 1.0 # blue self.bbmarker.color.a = 1.0 # alpha corner1 = Point() corner2 = Point() corner3 = Point() corner4 = Point() corner1.y = -self.bb_wid / 2000 corner1.x = 0 corner2.y = -self.bb_wid / 2000 corner2.x = self.bb_len / 1000 corner3.y = self.bb_wid / 2000 corner3.x = self.bb_len / 1000 corner4.y = self.bb_wid / 2000 corner4.x = 0 self.bbmarker.points.append(corner1) self.bbmarker.points.append(corner2) self.bbmarker.points.append(corner3) self.bbmarker.points.append(corner4) self.bbmarker.points.append(corner1) # for i in range(1,10000): # self.publisher_bblinelist.publish(self.bbmarker) self.sub_topic_arr = '/{}/obst_detect/posearray'.format(robot_name) self.subscriber_arr = message_filters.Subscriber( self.sub_topic_arr, PoseArray) # we MUST subscribe to the array for sure!! if (self.show_image): self.sub_topic = '/{}/camera_node/image/compressed'.format( robot_name) self.subscriber = message_filters.Subscriber( self.sub_topic, CompressedImage) if (self.show_marker and not (self.show_image)): self.subscriber_arr.registerCallback(self.marker_only_callback) else: self.ts = message_filters.TimeSynchronizer( [self.subscriber_arr, self.subscriber], 100) self.ts.registerCallback(self.callback)
## LETS PUBLISH THIS TO THE BROADCASTER tf_br.sendTransform((0.0, 0.0, 0.5), (q[0], q[1], q[2], q[3]), Time.now(), 'base_link', 'world') # publish the roll pitch yaw topic stamped_msg = SensorMsgStamped() stamped_msg.data = [rollA, pitchA, yawM] stamped_msg.header.stamp.secs = Time.now().secs stamped_msg.header.stamp.nsecs = Time.now().nsecs * 10**(-9) rpyAM_pub_stamped.publish(stamped_msg) if __name__ == "__main__": try: mag_sub = message_filters.Subscriber("/Magneto_topic_stamped", SensorMsgStamped) accel_sub = message_filters.Subscriber("/Accel_topic_stamped", SensorMsgStamped) combined_sub = message_filters.ApproximateTimeSynchronizer( [accel_sub, mag_sub], queue_size=5, slop=0.05) combined_sub.registerCallback(got_accel_mag) rospy.spin() except rospy.ROSInterruptException: pass ######################## # TO UNDERSTAND FUSION BETWEEN ACCELEROMETER AND MAGNETOMETER # Author : Kartik Prakash # Date : 7/Mar/2020 # STEPS: # 1. Get the raw accel values and the magnetometer values