def __init__(self): rospy.init_node('Base_position', anonymous=True) self.base_pub = rospy.Publisher('/odometry/filtered_map', Odometry, queue_size=1) self.br = TransformBroadcaster() trans = (0, 0, 0) rot = (0, 0, 0, 1) stamp = rospy.Time.now() self.br.sendTransform(trans, rot, stamp, "base_link", "map") self.Current_goal = [] self.Movedir = [0, 0, 0] self.VPose = Odometry() self.VPose.header.stamp = rospy.Time.now() self.VPose.header.frame_id = "map" self.VPose.pose.pose.position.x = float(0) self.VPose.pose.pose.position.y = float(0) self.VPose.pose.pose.position.z = float(0) self.VPose.pose.pose.orientation.x = float(0) self.VPose.pose.pose.orientation.y = float(0) self.VPose.pose.pose.orientation.z = float(0) self.VPose.pose.pose.orientation.w = float(1) rate = rospy.Rate(0.5) # 10hz while not rospy.is_shutdown(): rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.Find_goal, queue_size=1) if self.Current_goal == []: self.Movedir = [0, 0, 0] else: Movex = self.Current_goal.pose.position.x - self.VPose.pose.pose.position.x Movey = self.Current_goal.pose.position.y - self.VPose.pose.pose.position.y Movez = self.Current_goal.pose.position.z - self.VPose.pose.pose.position.z Movemag = math.sqrt((Movex)**2 + (Movey)**2) self.Movedir = [Movex, Movey, 0] print(self.Movedir) #self.Movedir = [1,0,0] self.VPose.header.stamp = rospy.Time.now() self.VPose.pose.pose.position.x += self.Movedir[0] self.VPose.pose.pose.position.y += self.Movedir[1] self.VPose.pose.pose.position.z += self.Movedir[2] self.br = TransformBroadcaster() trans = (self.VPose.pose.pose.position.x, self.VPose.pose.pose.position.y, self.VPose.pose.pose.position.z) rot = (self.VPose.pose.pose.orientation.x, self.VPose.pose.pose.orientation.y, self.VPose.pose.pose.orientation.z, self.VPose.pose.pose.orientation.w) self.br.sendTransform(trans, rot, self.VPose.header.stamp, "base_link", "map") #Pose.header.stamp,"base","map") self.base_pub.publish(self.VPose) rate.sleep()
def __init__(self): rospy.init_node("learn_transform") self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.possible_base_link_poses = [] self.baselink_averages = [] self.rate = rospy.Rate(20) self.markers = { "/PATTERN_1": Pose( [-0.56297, 0.11, 0.0035], [0.0, 0.0, 0.0, 1.0] ), "/PATTERN_2": Pose( [-0.45097, 0.11, 0.0035], [0.0, 0.0, 0.0, 1.0] ), "/PATTERN_3": Pose( [-0.34097, 0.0, 0.0035], [0.0, 0.0, 0.0, 1.0] ), "/PATTERN_4": Pose( [-0.34097, -0.11, 0.0035], [0.0, 0.0, 0.0, 1.0] ), "/PATTERN_5": Pose( [-0.45097, -0.11, 0.0035], [0.0, 0.0, 0.0, 1.0] ), "/PATTERN_6": Pose( [-0.56297, 0.0, 0.0035], [0.0, 0.0, 0.0, 1.0] ) } self.run()
def __init__(self): # Public attributes if World.tf_listener is None: World.tf_listener = TransformListener() self.surface = None # Private attributes self._lock = threading.Lock() self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) rospy.wait_for_service(SERVICE_BB) self._bb_service = rospy.ServiceProxy( SERVICE_BB, FindClusterBoundingBox) self._object_action_client = actionlib.SimpleActionClient( ACTION_OBJ_DETECTION, UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo( 'Interactive object detection action server has responded.') # Setup other ROS machinery rospy.Subscriber( TOPIC_OBJ_RECOGNITION, GraspableObjectList, self.receive_object_info) rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker) # Init self.clear_all_objects()
def __init__(self, img_proc=None): super(LandmarkMethodROS, self).__init__(device_id_facedetection=rospy.get_param( "~device_id_facedetection", default="cuda:0")) self.subject_tracker = FaceEncodingTracker() if rospy.get_param( "~use_face_encoding_tracker", default=True) else SequentialTracker() self.bridge = CvBridge() self.__subject_bridge = SubjectListBridge() self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_nonrotated_link") self.tf_broadcaster = TransformBroadcaster() self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze") self.visualise_headpose = rospy.get_param("~visualise_headpose", default=True) self.pose_stabilizers = {} # Introduce scalar stabilizers for pose. try: if img_proc is None: tqdm.write("Wait for camera message") cam_info = rospy.wait_for_message("/camera_info", CameraInfo, timeout=None) self.img_proc = PinholeCameraModel() # noinspection PyTypeChecker self.img_proc.fromCameraInfo(cam_info) else: self.img_proc = img_proc if np.array_equal( self.img_proc.intrinsicMatrix().A, np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])): raise Exception( 'Camera matrix is zero-matrix. Did you calibrate' 'the camera and linked to the yaml file in the launch file?' ) tqdm.write("Camera message received") except rospy.ROSException: raise Exception("Could not get camera info") # multiple person images publication self.subject_pub = rospy.Publisher("/subjects/images", MSG_SubjectImagesList, queue_size=3) # multiple person faces publication for visualisation self.subject_faces_pub = rospy.Publisher("/subjects/faces", Image, queue_size=3) Server(ModelSizeConfig, self._dyn_reconfig_callback) # have the subscriber the last thing that's run to avoid conflicts self.color_sub = rospy.Subscriber("/image", Image, self.process_image, buff_size=2**24, queue_size=3)
def __init__(self, use_dummy_transform=False): rospy.init_node('star_center_positioning_node') if use_dummy_transform: self.odom_frame_name = "odom_dummy" else: self.odom_frame_name = "odom" self.marker_locators = {} self.add_marker_locator( MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0)) self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi)) self.add_marker_locator( MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0)) self.add_marker_locator( MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0)) self.pose_correction = rospy.get_param('~pose_correction', 0.0) self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers) self.odom_sub = rospy.Subscriber("/odom", Odometry, self.process_odom, queue_size=10) self.star_pose_pub = rospy.Publisher("STAR_pose", PoseStamped, queue_size=10) self.continuous_pose = rospy.Publisher("STAR_pose_continuous", PoseStamped, queue_size=10) # self.star_pose_euler_angle_pub = rospy.Publisher("STAR_pose_euler_angle",Vector3,queue_size=10) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster()
def __init__(self, linear_covariance=0.1, angular_covariance=0.5, tf_frame='odom', camera_fov=1.0): """ :param linear_covariance: (meters) the linear covariance associated with a tag detection :param angular_covariance: (radians) the angular covariance of a tag :param tf_frame (str): The TF frame of the map / world. :param camera_fov (float): The FOV radius of the camera, in meters. """ self.tf = TransformListener() self.pub = rospy.Publisher('visible_roombas', RoombaList, queue_size=0) self.tf_pub = TransformBroadcaster() cov = [0] * 36 cov[0] = cov[7] = cov[14] = linear_covariance cov[21] = cov[28] = cov[35] = angular_covariance self.covariance = cov self.fov = camera_fov self.map_frame = tf_frame # Negates x and y coordinates of tag detections self.apply_apriltag_fix = True rospy.Subscriber('tag_detections', AprilTagDetectionArray, self.on_tags)
def __init__(self): self.bfp = True self.robot = robot_interface.Robot_Interface() self.url = 'http://172.31.76.30:80/ThinkingQ/' self.joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] self.tf_listener = TransformListener() trfm = Transformer() self.r2b = trfm.transform_matrix_of_frames( 'head_camera_rgb_optical_frame', 'base_link') self.model = load_model("./model/0.988.h5") self.br = TransformBroadcaster() self.move_group = MoveGroupInterface("arm", "base_link") # self.pose_group = moveit_commander.MoveGroupCommander("arm") self.cam = camera.RGBD() self.position_cloud = None while True: try: cam_info = self.cam.read_info_data() if cam_info is not None: break except: rospy.logerr('camera info not recieved') self.pcm = PCM() self.pcm.fromCameraInfo(cam_info)
def __init__(self, device_id_gaze, model_files): super(GazeEstimatorROS, self).__init__(device_id_gaze, model_files) self.bridge = CvBridge() self.subjects_bridge = SubjectListBridge() self.tf_broadcaster = TransformBroadcaster() self.tf_listener = TransformListener() self.tf_prefix = rospy.get_param("~tf_prefix", "gaze") self.headpose_frame = self.tf_prefix + "/head_pose_estimated" self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_ros_frame") self.image_subscriber = rospy.Subscriber("/subjects/images", MSG_SubjectImagesList, self.image_callback, queue_size=3, buff_size=2**24) self.subjects_gaze_img = rospy.Publisher("/subjects/gazeimages", Image, queue_size=3) self.visualise_eyepose = rospy.get_param("~visualise_eyepose", default=True) self._last_time = rospy.Time().now()
def __init__(self, use_dummy_transform=False): rospy.init_node('star_center_positioning_node') if use_dummy_transform: self.odom_frame_name = "odom_dummy" else: self.odom_frame_name = "odom" self.marker_locators = {} self.add_marker_locator(MarkerLocator(0, (0.0, 0.0), 0)) self.add_marker_locator(MarkerLocator(1, (1.4 / 1.1, 2.0 / 1.1), 0)) self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers) self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10) self.star_pose_pub = rospy.Publisher("STAR_pose", PoseStamped, queue_size=10) self.continuous_pose = rospy.Publisher("STAR_pose_continuous", PoseStamped, queue_size=10) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster()
def __init__(self): self.vel_pub = rospy.Publisher( '/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=0) self.odom_pub = rospy.Publisher('/odometry', Odometry, queue_size=10) self.tfb = TransformBroadcaster() rospy.sleep(0.5) startpose = PoseStamped() startpose.header.stamp = rospy.Time.now() startpose.header.frame_id = 'odom' startpose.pose.orientation.w = 1 self.publish_pose(startpose, TwistWithCovariance()) rospy.loginfo("Published initial tf") self.set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) self.set_rate = rospy.ServiceProxy('/mavros/set_stream_rate', StreamRate) self.land = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) self.takeoff = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL) rospy.loginfo('Services connected') self.vel = Twist() self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.on_vel) self.takeoff_sub = rospy.Subscriber('/takeoff', Empty, self.on_takeoff) self.land_sub = rospy.Subscriber('/land', Empty, self.on_land) self.last_pose = PoseStamped() self.position_sub = rospy.Subscriber('/mavros/global_position/local', Odometry, self.on_pose)
def __init__(self, name='robot_00', x=5.0, y=5.0, theta=0.0): rospy.init_node('turtlesim_to_odom') self.name = name rospy.wait_for_service('/kill') kill = rospy.ServiceProxy('/kill',Kill) if name == 'robot_00': try: kill('turtle1') except: pass try: kill(name) except: pass x = float(x) y = float(y) theta = float(theta) rospy.wait_for_service('/spawn') spawn = rospy.ServiceProxy('/spawn',Spawn) try: spawn(x, y, theta, name) except: pass rospy.Subscriber('pose', TurtlePose, self.pose_callback, queue_size=1) self.odom_pub = rospy.Publisher('odometry', Odometry, queue_size=1) self.tfb = TransformBroadcaster()
def echo_socket(ws): b = TransformBroadcaster() f = open("orientation.txt", "a") while True: message = ws.receive() orient_data = message.split(',') orient_data = [float(data) for data in orient_data] stamped_msg = SensorMsgStamped() stamped_msg.data = orient_data stamped_msg.header.stamp.secs = Time.now().secs stamped_msg.header.stamp.nsecs = Time.now().nsecs orient_pub_stamped.publish(stamped_msg) ### Publish to Pose topic for visualization ### q = quaternion_from_euler(orient_data[1], orient_data[2], orient_data[3]) pose_msg = Pose() pose_msg.orientation.x = q[0] pose_msg.orientation.y = q[1] pose_msg.orientation.z = q[2] pose_msg.orientation.w = q[3] pose_pub.publish(pose_msg) b.sendTransform((1, 1, 1), (q[0], q[1], q[2], q[3]), Time.now(), 'child_link', 'base_link') ### END HERE ### print("[INFO:] Orientation{}".format(orient_data)) ws.send(message) print >> f, message f.close()
def __init__(self): # once everything is setup initialized will be set to true self.initialized = False # initialize this particle filter node rospy.init_node('turtlebot3_particle_filter') # set the topic names and frame names self.base_frame = "base_footprint" self.map_topic = "map" self.odom_frame = "odom" self.scan_topic = "scan" # inialize our map self.map = OccupancyGrid() # create LikelihoodField object self.likelihood_field = LikelihoodField() # the number of particles used in the particle filter self.num_particles = 10000 # initialize the particle cloud array self.particle_cloud = [] # initialize the estimated robot pose self.robot_estimate = Pose() # set threshold values for linear and angular movement before we preform an update self.lin_mvmt_threshold = 0.2 self.ang_mvmt_threshold = (np.pi / 6) self.odom_pose_last_motion_update = None # Setup publishers and subscribers # publish the current particle cloud self.particles_pub = rospy.Publisher("particle_cloud", PoseArray, queue_size=10) # publish the estimated robot pose self.robot_estimate_pub = rospy.Publisher("estimated_robot_pose", PoseStamped, queue_size=10) # subscribe to the map server rospy.Subscriber(self.map_topic, OccupancyGrid, self.get_map) # subscribe to the lidar scan from the robot rospy.Subscriber(self.scan_topic, LaserScan, self.robot_scan_received) # enable listening for and broadcasting corodinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() # sleep to get map data before initializing particle cloud rospy.sleep(1) # intialize the particle cloud self.initialize_particle_cloud() self.initialized = True
def __init__(self, continuous_estimation=False, joint_states_topic="/joint_states", visual_tags_enabled=True): super(Estimation, self).__init__(context="estimation") self.locked_joints = [] self.tf_pub = TransformBroadcaster() self.tf_root = "world" self.mutex = Lock() self.robot_name = rospy.get_param("~robot_name", "") self.last_stamp_is_ready = False self.last_stamp = rospy.Time.now() self.last_visual_tag_constraints = list() self.current_stamp = rospy.Time.now() self.current_visual_tag_constraints = list() self.visual_tags_enabled = visual_tags_enabled self.continuous_estimation(SetBoolRequest(continuous_estimation)) self.estimation_rate = 50 # Hz self.subscribers = ros_tools.createSubscribers(self, "/agimus", self.subscribersDict) self.publishers = ros_tools.createPublishers("/agimus", self.publishersDict) self.services = ros_tools.createServices(self, "/agimus", self.servicesDict) self.joint_state_subs = rospy.Subscriber(joint_states_topic, JointState, self.get_joint_state)
def main(): rospy.init_node('Odometria_Xbox') b = TransformBroadcaster() translation = (0.0, 0.0, 0.0) rotation = (0.0, 0.0, 0.0, 1.0) #quaternion rate = rospy.Rate(5) # 5hz yant = 0.0 xant = 0.0 while not rospy.is_shutdown(): #y = math.degrees(math.asin(joy.leftX())) #+ yant y = joy.leftX() + yant x = joy.leftY() + xant translation = (x, 0.0, 0.0) rotation = tf.transformations.quaternion_from_euler(0, 0, y) yant = y xant = x #print(y) b.sendTransform(translation, rotation, Time.now(), 'camera_link', 'odom') rate.sleep()
def __init__(self): self.listener = tf.TransformListener() self.cells = [] cell_names = rospy.get_param("cells", ["n1", "n2"]) for cell in cell_names: self.cells.append( ArtCellCalibration(cell, '/art/' + cell + '/ar_pose_marker', '/' + cell + '_kinect2_link', '/marker_detected', '/' + cell_names[0] + '_kinect2_link', '/art/' + cell + '/kinect2/qhd/points', self.listener)) # self.cells.append(ArtRobotCalibration('pr2', '/pr2/ar_pose_marker', # '/odom_combined', '/marker_detected', # '/pr2/points')) self.calibrated_pub = rospy.Publisher('/art/system/calibrated', Bool, queue_size=10, latch=True) self.calibrated = Bool() self.calibrated.data = False self.calibrated_sended = False self.calibrated_pub.publish(self.calibrated) self.recalibrate_cell_service = rospy.Service( "/art/system/calibrate_cell", RecalibrateCell, self.recalibrate_cell_cb) self.broadcaster = TransformBroadcaster()
def handle_model_state(msg): global last_time br = TransformBroadcaster() model_names = msg.name # rospy.logerr_throttle(1,f'{str(model_names)}') try: # idx = model_names.index(tfPrefix) time = rospy.Time.now() if time == last_time: return idx = -1 for i in range(len(model_names)): # rospy.logerr_throttle_identical(1,f'{tfPrefix}:{model_names[i]}') if tfPrefix == model_names[i]: idx = i # rospy.logwarn('FOUND') break pose = msg.pose[idx] br.sendTransform((pose.position.x, pose.position.y, pose.position.z), (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), time, tfPrefix + "/" + "base_link", "world") last_time = time except IndexError: rospy.logwarn_throttle(10, f'Cannot find Gazebo model state {tfPrefix}')
def main(): rospy.init_node('my_broadcaster') b = TransformBroadcaster() translation = (0.0, 0.0, 0.0) rotation = (0.0, 0.0, 0.0, 1.0) rate = rospy.Rate(5) # 5hz x, y = 0.0, 0.0 while not rospy.is_shutdown(): if x >= 2: x, y = 0.0, 0.0 x += 0.1 y += 0.1 translation = (x, y, 0.0) b.sendTransform(translation, rotation, Time.now(), '/odom', '/world') b.sendTransform(translation, rotation, Time.now(), '/laser', '/chassis') b.sendTransform(translation, rotation, Time.now(), '/camera1', '/chassis') rate.sleep()
def __init__(self): rospy.init_node('left_utilitiy_frame_source') self.updater = rospy.Service('l_utility_frame_update', FrameUpdate, self.update_frame) self.tfb = TransformBroadcaster()
def __init__(self): self.tl = TransformListener() self.br = TransformBroadcaster()
def __init__(self): """ init method """ self.odometry = Odometry() self.odometry_broadcaster = TransformBroadcaster() self.odometry_transform = TransformStamped() self.x = 0.0 self.y = 0.0 self.th = 0.0 self.vx = 0 self.vy = 0 self.vth = 0 self.seq = 0 self.dt = 0 self.delta_x = 0 self.delta_y = 0 self.delta_th = 0 self.odometry_quaternion = trans.quaternion_from_euler(0, 0, self.th) self.current_time = rospy.get_time() self.last_time = rospy.get_time()
def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 500 # the number of particles to use self.d_thresh = 0.1 # the amount of linear movement before performing an update self.a_thresh = math.pi / 12 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] # request the map from the map server rospy.wait_for_service('static_map') try: map_server = rospy.ServiceProxy('static_map', GetMap) map = map_server().map print map.info.resolution except: print "Service call failed!" # initializes the occupancyfield which contains the map self.occupancy_field = OccupancyField(map) print "initialized" self.initialized = True
def update_transform(self, pose, target_frame='base_laser_link'): # Updates the transform between the map frame and the odom frame br = TransformBroadcaster() br.sendTransform((pose[0], pose[1], 0), quaternion_from_euler(0, 0, pose[2]+math.pi), rospy.get_rostime(), target_frame, 'map')
def move_robot_model_rotate(): translation = (0.0, 0.0, 0.0) rotation = (0.0, 3.0, 0.0, 1.0) b = TransformBroadcaster() b.sendTransform(translation, rotation, Time.now(), 'base_link', '/map') b.sendTransform(translation, rotation, Time.now(), 'camera_link', '/map') b.sendTransform(translation, rotation, Time.now(), 'camera_link_normalized', '/map')
def __init__(self): rospy.init_node("iidre_uwb_xyz_publisher") self.tfb = TransformBroadcaster() self.serial = None self.device_name = rospy.get_param("name", "uwb") self.device_port = rospy.get_param("port", "/dev/ttyACM0") self.device_frame_id = rospy.get_param("frame_id", "map") self.publish_anchors = rospy.get_param("publish_anchors", True)
def __init__(self): self.tf = TransformListener() self.tfb = TransformBroadcaster() self.active = True self.head_pose = PoseStamped() self.goal_pub = rospy.Publisher('goal_pose', PoseStamped) rospy.Subscriber('/head_center', PoseStamped, self.head_pose_cb) rospy.Service('/point_mirror', PointMirror, self.point_mirror_cb)
def __init__(self): self.image_height = rospy.get_param("~image_height", 36) self.image_width = rospy.get_param("~image_width", 60) self.bridge = CvBridge() self.subjects_bridge = SubjectListBridge() self.tf_broadcaster = TransformBroadcaster() self.tf_listener = TransformListener() self.use_last_headpose = rospy.get_param("~use_last_headpose", True) self.tf_prefix = rospy.get_param("~tf_prefix", "gaze") self.last_phi_head, self.last_theta_head = None, None self.rgb_frame_id_ros = rospy.get_param("~rgb_frame_id_ros", "/kinect2_nonrotated_link") self.headpose_frame = self.tf_prefix + "/head_pose_estimated" self.device_id_gazeestimation = rospy.get_param( "~device_id_gazeestimation", default="/gpu:0") with tensorflow.device(self.device_id_gazeestimation): config = tensorflow.ConfigProto(inter_op_parallelism_threads=1, intra_op_parallelism_threads=1) if "gpu" in self.device_id_gazeestimation: config.gpu_options.allow_growth = True config.gpu_options.per_process_gpu_memory_fraction = 0.3 config.log_device_placement = False self.sess = tensorflow.Session(config=config) set_session(self.sess) model_files = rospy.get_param("~model_files") self.models = [] for model_file in model_files: tqdm.write('Load model ' + model_file) model = load_model(os.path.join( rospkg.RosPack().get_path('rt_gene'), model_file), custom_objects={ 'accuracy_angle': accuracy_angle, 'angle_loss': angle_loss }) # noinspection PyProtectedMember model._make_predict_function( ) # have to initialize before threading self.models.append(model) tqdm.write('Loaded ' + str(len(self.models)) + ' models') self.graph = tensorflow.get_default_graph() self.image_subscriber = rospy.Subscriber('/subjects/images', MSG_SubjectImagesList, self.image_callback, queue_size=3) self.subjects_gaze_img = rospy.Publisher('/subjects/gazeimages', Image, queue_size=3) self.average_weights = np.array([0.1, 0.125, 0.175, 0.2, 0.4]) self.gaze_buffer_c = {}
def __init__(self, config): rospy.loginfo("Start to intialize moveit_control_interface") # Config self.cfg = config.cfg # TF self.tf = TransformListener() self.br = TransformBroadcaster() rospy.Subscriber(self.cfg['topicMoveGroupResult'], MoveGroupActionResult, self.cb_move_group_result) rospy.Subscriber(self.cfg['topicTrajectoryExecutionResult'], ExecuteTrajectoryActionResult, self.cb_trajectory_execution_result) # Wait for Moveit action server moveit_interface_found = False while not moveit_interface_found: try: self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander( self.cfg['move_group_name']) moveit_interface_found = True except Exception as e: rospy.logerr(e.message) moveit_interface_found = False rospy.logerr("Retrying to connect to MoveIt action server ...") rospy.signal_shutdown('No MoveIt interface found') return # Create an inverse mapping of the joint names self.active_joints = self.group.get_active_joints() rospy.loginfo("============ Active joints: %d" % len(self.active_joints)) self.active_joints_id = {} i = 0 for joint_name in self.active_joints: self.active_joints_id[i] = joint_name i += 1 rospy.loginfo(self.active_joints_id) # Joint/Goal tolerances for motion planning: rospy.loginfo( "============ Goal tolerances: joint, position, orientation") self.group.set_goal_position_tolerance( self.cfg['goal_position_tolerance']) self.group.set_goal_joint_tolerance(self.cfg['goal_joint_tolerance']) self.group.set_goal_orientation_tolerance( self.cfg['goal_orientation_tolerance']) rospy.loginfo("Joint-, position-, orientation tolerance: ") rospy.loginfo( self.group.get_goal_tolerance() ) # Return a tuple of goal tolerances: joint, position and orientation. rospy.loginfo("============ Reference frame for poses of end-effector") rospy.loginfo(self.group.get_pose_reference_frame())
def __init__(self): rospy.init_node('republish_tag_tf') self.tf = TransformListener() self.br = TransformBroadcaster() rospy.Subscriber("/tag_detections", AprilTagDetectionArray, self.tags_callback, queue_size=10) self.tag_detections = rospy.Publisher('/selective_grasping/tag_detections', Int16MultiArray, queue_size=10) self.detections_list = np.zeros(8)
def __init__(self): self.image_pub = rospy.Publisher("/postImage",Image,queue_size=50) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback) self.h = Header() self.tb = TransformBroadcaster() self.img_tf=TransformStamped()