コード例 #1
0
    def __init__(self, ns, ActionSpec):
        self.ns = ns
        self.last_status_msg = None

        try:
            a = ActionSpec()

            self.ActionSpec = ActionSpec
            self.ActionGoal = type(a.action_goal)
            self.ActionResult = type(a.action_result)
            self.ActionFeedback = type(a.action_feedback)
        except AttributeError:
            raise ActionException("Type is not an action spec: %s" %
                                  str(ActionSpec))

        self.pub_goal = rospy.Publisher(
            rospy.remap_name(ns) + '/goal', self.ActionGoal)
        self.pub_cancel = rospy.Publisher(
            rospy.remap_name(ns) + '/cancel', GoalID)

        self.manager = GoalManager(ActionSpec)
        self.manager.register_send_goal_fn(self.pub_goal.publish)
        self.manager.register_cancel_fn(self.pub_cancel.publish)

        self.status_sub = rospy.Subscriber(
            rospy.remap_name(ns) + '/status', GoalStatusArray, self._status_cb)
        self.result_sub = rospy.Subscriber(
            rospy.remap_name(ns) + '/result', self.ActionResult,
            self._result_cb)
        self.feedback_sub = rospy.Subscriber(
            rospy.remap_name(ns) + '/feedback', self.ActionFeedback,
            self._feedback_cb)
コード例 #2
0
def cal_from_tarfile(boards, tarname, mono = False, upload = False):
    if mono:
        calibrator = MonoCalibrator(boards)
    else:
        calibrator = StereoCalibrator(boards)

    calibrator.do_tarfile_calibration(tarname)

    print calibrator.ost()

    if upload: 
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
        else:
            set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)

            response1 = set_camera_info_service(info[0])
            response2 = set_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
コード例 #3
0
    def __init__(self,
                 boards,
                 service_check=True,
                 synchronizer=message_filters.TimeSynchronizer,
                 flags=0,
                 pattern=Patterns.Chessboard,
                 camera_name='',
                 detection='cv2',
                 output='yaml',
                 checkerboard_flags=0):
        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._detection = detection
        self._output = output
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        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 = deque([], 1)
        self.q_stereo = deque([], 1)

        self.c = 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()
コード例 #4
0
def cal_from_tarfile(boards, tarname, mono = False, upload = False):
    if mono:
        calibrator = MonoCalibrator(boards)
    else:
        calibrator = StereoCalibrator(boards)

    calibrator.do_tarfile_calibration(tarname)

    print calibrator.ost()

    if upload: 
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
        else:
            set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)

            response1 = set_left_camera_info_service(info[0])
            response2 = set_right_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
コード例 #5
0
    def __init__(self, size, dim):

        self.chess_size = size
        self.dim = dim

        left_ns = rospy.remap_name('left')
        range_ns = rospy.remap_name('range')
        limg_sub = message_filters.Subscriber(left_ns + '/image_rect',
                                              sensor_msgs.msg.Image)
        rimg_sub = message_filters.Subscriber(range_ns + '/image_rect',
                                              sensor_msgs.msg.Image)
        linfo_sub = message_filters.Subscriber(left_ns + '/camera_info',
                                               sensor_msgs.msg.CameraInfo)
        rinfo_sub = message_filters.Subscriber(range_ns + '/camera_info',
                                               sensor_msgs.msg.CameraInfo)

        ts = message_filters.TimeSynchronizer(
            [limg_sub, linfo_sub, rimg_sub, rinfo_sub], 16)
        ts.registerCallback(self.queue_cr)
        self.bridge = cv_bridge.CvBridge()

        self.frame_list = []
        self.static_pose = None
        self.result_list = []
        self.last_err = 0
コード例 #6
0
    def __init__(self, ns, action_server, state_publish_cb):
        self.ns = ns
        self.last_status_msg = None
        self.action_server   = action_server
        self.is_running      = False

        try:
            a = FollowJointTrajectoryAction()

            self.ActionSpec      = FollowJointTrajectoryAction
            self.ActionGoal      = type(a.action_goal)
            self.ActionResult    = type(a.action_result)
            self.ActionFeedback  = type(a.action_feedback)
            self.JointTrajectory = JointTrajectory

        except AttributeError:
            raise ActionException("Type is not an action spec: %s" % str(self.ActionSpec))

        print "Defining topics for ",ns, "  --> ",rospy.remap_name(ns)

        self.pub_command   = rospy.Publisher(rospy.remap_name(ns) + '/command', self.JointTrajectory, queue_size=1)

        action_name = rospy.remap_name(ns)+"/follow_joint_trajectory";
        self.pub_goal   = rospy.Publisher(action_name + '/goal', self.ActionGoal, queue_size=1)
        self.pub_cancel = rospy.Publisher(action_name + '/cancel', GoalID, queue_size=1)

        self.manager = GoalManager(self.ActionSpec)
        self.manager.register_send_goal_fn(self.pub_goal.publish)
        self.manager.register_cancel_fn(self.pub_cancel.publish)

        self.status_sub   = rospy.Subscriber(action_name + '/status', GoalStatusArray, self._status_cb)
        self.result_sub   = rospy.Subscriber(action_name + '/result', self.ActionResult, self._result_cb)
        self.feedback_sub = rospy.Subscriber(action_name + '/feedback', self.ActionFeedback, self._feedback_cb)
        self.state_sub    = rospy.Subscriber(rospy.remap_name(ns) + '/state', JointTrajectoryControllerState, state_publish_cb)
コード例 #7
0
    def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
                 pattern=Patterns.Chessboard, camera_name='', detection='cv2', output='yaml', checkerboard_flags = 0,
                 min_good_enough = 40):
        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._detection = detection
        self._output = output
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        self._min_good_enough = min_good_enough
        rospack = rospkg.RosPack()
        pkg_path = rospack.get_path('autoware_camera_lidar_calibrator')
        self._autoware_image = cv2.imread( path.join(pkg_path, 'docs/autoware_logo.jpg'), cv2.IMREAD_UNCHANGED)
        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 = deque([], 1)
        self.q_stereo = deque([], 1)

        self.c = 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()
コード例 #8
0
ファイル: cameracalibrator.py プロジェクト: mycruet/MVROS
    def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
        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
        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)

        calibrateService = rospy.Service('calibrates', Calibrate, self.do_calibrate)
        
        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 = deque([], 1)
        self.q_stereo = deque([], 1)

        self.c = None

        self.mth = ConsumerThread(self.q_mono, self.handle_monocular)
        self.mth.setDaemon(True)
        self.mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()
コード例 #9
0
    def __init__(self):
        Node("multi_interface_roam")
        self.interface_selector = interface_selector.InterfaceSelector()
        self.reconfig_server = dynamic_reconfigure.server.Server(
            MultiInterfaceRoamConfig, self.reconfigure)
        self._interfaces = self.interface_selector.interfaces.values()
        self.hostname = os.uname()[1]

        # Prepare topics to publish
        pub_namespace = rospy.remap_name('wifi')
        self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
        self.ap_pub = rospy.Publisher(pub_namespace + "/accesspoint",
                                      AccessPoint)
        self.status_pub = rospy.Publisher(pub_namespace + "/status",
                                          MultiInterfaceStatus)
        self.iface_id_pub = rospy.Publisher(pub_namespace +
                                            '/current_iface_id',
                                            Int32,
                                            latch=True)
        self._wireless_interfaces = [
            i for i in self._interfaces
            if i.__class__ == interface.WirelessInterface
        ]
        self.all_ap_pub = dict((
            iface,
            rospy.Publisher(pub_namespace + "/" + iface.iface +
                            "/accesspoint", AccessPoint))
                               for iface in self._wireless_interfaces)

        # Kick off publication updates.
        self.interface_selector.update_event.subscribe_repeating(
            self._publish_status)
コード例 #10
0
def talker():

    Nome_topico = 'Conversa'
    frequency = 10

    #setup do publicador: as mensagens já n ão são do tipo string, mas do Num (mensagem)
    pub = rospy.Publisher(Nome_topico, Mensagem_Dog, queue_size=10)
    rospy.init_node(
        'publisher', anonymous=False
    )  #Posso colocar o anonymous a false, mas depois pode haver colisões de canais. O publisher pode mudar para Samuel
    rate = rospy.Rate(frequency)

    #Para mudar o valor do parâmetro, no terminal:  rosparam set /highlight_text_color YELLOW

    #Neste caso no Fore tenho usar esta função pois é um dicionário e cor é uma string
    while not rospy.is_shutdown():
        # Get value of parameter (first i had to make de set of the parame.)
        cor = rospy.get_param(
            '~highlight_text_color'
        )  #Como estou a usar um parametro privado( que gravei com o nome publisher) meto o til e assim reconhece esse nome
        hello_str = "Publicar: " + getattr(Fore, cor) + str(
            dog_msg) + Fore.RESET + " on topic: " + rospy.remap_name(
                Nome_topico)  #Altera a string de debug o nome do topico
        rospy.loginfo(hello_str)
        pub.publish(
            dog_msg
        )  #Aqui tenho de enviar a minha mensagem do Num que foi criada à parte.
        rate.sleep()
コード例 #11
0
    def __init__(self, cameraInfoFile):
        """ Overlay lidar points onto images by setting up the camera model, camera intrinsics, and lidar-camera extrinsics.
        ===============
        cameraInfoFile : an opened yaml file that stores camera intrinsics and other params
            used to initialize the cameraInfo which is used to help cameraModel reproject lidar points to image space. 
        """
        self.cameraParams = yaml.load(cameraInfoFile)
        self.cameraInfo = CameraInfo()
        self._fillCameraInfo()
        self.cameraModel = PinholeCameraModel()
        self.cameraModel.fromCameraInfo(self.cameraInfo)

        print('Distortion model:', self.cameraInfo.distortion_model)

        self.bridge = cv_bridge.CvBridge()
        # Get transformation/extrinsics between lidar (velodyne frame) and camera (world frame)
        self.tf = tf.TransformListener()

        # Get the topics' names to subscribe or publish
        self.inImageName = rospy.resolve_name('image')
        self.outImageName = rospy.resolve_name('lidar_image')
        self.lidarName = rospy.remap_name('lidar')

        # Create subscribers and publishers
        self.subImage = rospy.Subscriber(self.inImageName,
                                         Image,
                                         callback=self.imageCallback,
                                         queue_size=1)
        self.lidar = rospy.Subscriber(self.lidarName,
                                      PointCloud2,
                                      callback=self.lidarCallback,
                                      queue_size=1)
        self.pubImage = rospy.Publisher(self.outImageName, Image, queue_size=1)

        self.lidarPoints = None
コード例 #12
0
    def __init__(self):
        # format of input audio data
        self.sample_rate = rospy.get_param("~sample_rate", 16000)
        self.sample_width = rospy.get_param("~sample_width", 2L)
        # language of STT service
        self.language = rospy.get_param("~language", "ja-JP")
        # ignore voice input while the robot is speaking
        self.self_cancellation = rospy.get_param("~self_cancellation", True)
        # time to assume as SPEAKING after tts service is finished
        self.tts_tolerance = rospy.Duration.from_sec(
            rospy.get_param("~tts_tolerance", 1.0))

        self.recognizer = SR.Recognizer()

        self.tts_action = None
        self.last_tts = None
        self.is_canceling = False
        if self.self_cancellation:
            self.tts_action = actionlib.SimpleActionClient(
                "sound_play", SoundRequestAction)
            if self.tts_action.wait_for_server(rospy.Duration(5.0)):
                self.tts_timer = rospy.Timer(rospy.Duration(0.1),
                                             self.tts_timer_cb)
            else:
                rospy.logerr("action '%s' is not initialized." %
                             rospy.remap_name("sound_play"))
                self.tts_action = None

        self.pub_speech = rospy.Publisher("speech_to_text",
                                          SpeechRecognitionCandidates,
                                          queue_size=1)
        self.sub_audio = rospy.Subscriber("audio", AudioData, self.audio_cb)
コード例 #13
0
    def __init__(self):
        # format of input audio data
        self.sample_rate = rospy.get_param("~sample_rate", 16000)
        self.sample_width = rospy.get_param("~sample_width", 2L)
        # language of STT service
        self.language = rospy.get_param("~language", "en-US")
        self.asr_engine = rospy.get_param("~asr_engine", "google_legacy_single_utterance")
        # ignore voice input while the robot is speaking
        self.self_cancellation = rospy.get_param("~self_cancellation", True)
        # time to assume as SPEAKING after tts service is finished
        self.tts_tolerance = rospy.Duration.from_sec(
            rospy.get_param("~tts_tolerance", 1.0))

        self.recognizer = PepperProjectRecognizer() #SR.Recognizer()

        self.tts_action = None
        self.last_tts = None
        self.is_canceling = False
        if self.self_cancellation:
            self.tts_action = actionlib.SimpleActionClient(
                "sound_play", SoundRequestAction)
            if self.tts_action.wait_for_server(rospy.Duration(5.0)):
                self.tts_timer = rospy.Timer(rospy.Duration(0.1), self.tts_timer_cb)
            else:
                rospy.logerr("action '%s' is not initialized." % rospy.remap_name("sound_play"))
                self.tts_action = None

        self.pub_speech = rospy.Publisher(
            "speech_to_text", SpeechRecognitionCandidates, queue_size=1)
        #confusing: ros launch file contains: <remap from="audio" to="speech_audio"/>
        #so actually here we are subscribing to speech_audio, which makes much more sense - if you just read the python code
        #and do not know about the remapping in the ros launch file, nothing makes sense anymore. hours were lost.
        self.sub_audio = rospy.Subscriber("audio", AudioData, self.audio_cb)
コード例 #14
0
    def __init__(self, topic, callback=None, encoding="bgr8", queue_size=1):
        '''Calls $callback on each image every time a new image is published on $topic
        Assumes topic of type "sensor_msgs/Image"
        This behaves like a conventional subscriber, except handling the additional image conversion
        '''
        if callback is None:

            def callback(im):
                return setattr(self, 'last_image', im)

        self.encoding = encoding
        self.camera_info = None
        self.last_image_header = None
        self.last_image_time = None
        self.last_image = None
        self.im_sub = rospy.Subscriber(topic,
                                       Image,
                                       self.convert,
                                       queue_size=queue_size)

        root_topic, image_subtopic = path.split(rospy.remap_name(topic))
        self.info_sub = rospy.Subscriber(root_topic + '/camera_info',
                                         CameraInfo,
                                         self.info_cb,
                                         queue_size=queue_size)

        self.bridge = CvBridge()
        self.callback = callback
コード例 #15
0
    def __init__(self):
        # format of input audio data
        self.sample_rate = rospy.get_param("~sample_rate", 16000)
        self.sample_width = rospy.get_param("~sample_width", 2L)
        # language of STT service
        self.language = rospy.get_param("~language", "ja-JP")
        # ignore voice input while the robot is speaking
        self.self_cancellation = rospy.get_param("~self_cancellation", True)
        # time to assume as SPEAKING after tts service is finished
        self.tts_tolerance = rospy.Duration.from_sec(
            rospy.get_param("~tts_tolerance", 1.0))

        self.recognizer = SR.Recognizer()

        self.tts_action = None
        self.last_tts = None
        self.is_canceling = False
        if self.self_cancellation:
            self.tts_action = actionlib.SimpleActionClient(
                "sound_play", SoundRequestAction)
            if self.tts_action.wait_for_server(rospy.Duration(5.0)):
                self.tts_timer = rospy.Timer(rospy.Duration(0.1), self.tts_timer_cb)
            else:
                rospy.logerr("action '%s' is not initialized." % rospy.remap_name("sound_play"))
                self.tts_action = None

        self.pub_speech = rospy.Publisher(
            "speech_to_text", SpeechRecognitionCandidates, queue_size=1)
        self.sub_audio = rospy.Subscriber("audio", AudioData, self.audio_cb)
コード例 #16
0
    def __init__(self, ns, ActionSpec):
        self.ns = ns
        self.last_status_msg = None

        try:
            a = ActionSpec()

            self.ActionSpec = ActionSpec
            self.ActionGoal = type(a.action_goal)
            self.ActionResult = type(a.action_result)
            self.ActionFeedback = type(a.action_feedback)
        except AttributeError:
            raise ActionException("Type is not an action spec: %s" %
                                  str(ActionSpec))

        self.pub_queue_size = rospy.get_param(
            'actionlib_client_pub_queue_size', 10)
        if self.pub_queue_size < 0:
            self.pub_queue_size = 10
        self.pub_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal',
                                        self.ActionGoal,
                                        queue_size=self.pub_queue_size)
        self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel',
                                          GoalID,
                                          queue_size=self.pub_queue_size)

        self.manager = GoalManager(ActionSpec)
        self.manager.register_send_goal_fn(self.pub_goal.publish)
        self.manager.register_cancel_fn(self.pub_cancel.publish)

        self.sub_queue_size = rospy.get_param(
            'actionlib_client_sub_queue_size', -1)
        if self.sub_queue_size < 0:
            self.sub_queue_size = None
        self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status',
                                           GoalStatusArray,
                                           callback=self._status_cb,
                                           queue_size=self.sub_queue_size)
        self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result',
                                           self.ActionResult,
                                           callback=self._result_cb,
                                           queue_size=self.sub_queue_size)
        self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) +
                                             '/feedback',
                                             self.ActionFeedback,
                                             callback=self._feedback_cb,
                                             queue_size=self.sub_queue_size)
コード例 #17
0
    def __init__(self, name):
        self._lock = threading.Lock()
        self._action_name = name
        self._board = None
        
        self._imageSub = rospy.Subscriber("image_raw", Image, self.img_callback)
        rospy.loginfo("[%s] Subscribing to: %s" % (rospy.get_name(), rospy.remap_name("image_raw")))
        self._featurePub = rospy.Publisher("features", CalibrationPattern)
        self._calImagePub = rospy.Publisher("calibration_image", Image)
        rospy.loginfo("[%s] Publishing features to: %s" % (rospy.get_name(), rospy.remap_name("features")))
        rospy.loginfo("[%s] Publishing calibration_image to: %s" % (rospy.get_name(), rospy.remap_name("calibration_image")))
        
        self._server = actionlib.SimpleActionServer(self._action_name, ConfigAction, None, False)
        self._server.register_goal_callback(self.goal_callback)
        self._server.register_preempt_callback(self.preempt_callback)

        self._server.start()
コード例 #18
0
    def initialize(self):
          self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=50);
          self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=50);
          self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=50);

          self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback);

          self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback);

          #read the frequency with which to publish status from the parameter server
          #if not specified locally explicitly, use search param to find actionlib_status_frequency
          resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
          if rospy.has_param(resolved_status_frequency_name):
              self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0);
              rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
          else:
              search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
              if search_status_frequency_name is None:
                  self.status_frequency = 5.0
              else:
                  self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)

          status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0);
          self.status_list_timeout = rospy.Duration(status_list_timeout);


          self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async,self.status_frequency) );
          self.status_timer.start();
コード例 #19
0
ファイル: action_server.py プロジェクト: strawlab/ros_common
    def initialize(self):
          self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True);
          self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult);
          self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback);

          self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback);

          self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback);

          #read the frequency with which to publish status from the parameter server
          #if not specified locally explicitly, use search param to find actionlib_status_frequency
          resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
          if rospy.has_param(resolved_status_frequency_name):
              self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0);
              rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
          else:
              search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
              if search_status_frequency_name is None:
                  self.status_frequency = 5.0
              else:
                  self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)

          status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0);
          self.status_list_timeout = rospy.Duration(status_list_timeout);


          self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async,self.status_frequency) );
          self.status_timer.start();
コード例 #20
0
 def init_sound(self):
     if self.self_cancellation:
         self.tts_action = actionlib.SimpleActionClient(
             "sound_play", SoundRequestAction)
         if self.tts_action.wait_for_server(rospy.Duration(5.0)):
             self.tts_timer = rospy.Timer(rospy.Duration(0.1), self.tts_timer_cb)
         else:
             rospy.logerr("action '%s' is not initialized." % rospy.remap_name("sound_play"))
             self.tts_action = None
コード例 #21
0
    def __init__(self):

        self.listener = tf.TransformListener()

        self.image_sub = rospy.Subscriber('/camera/image_raw',sensor_msgs.Image, self.image_cb)
        self.sample_sub = rospy.Subscriber('%s/calibration_samples' % rospy.remap_name("camera"),std_msgs.Int16, self.sampled_cb)
        self.poses = []
        self.last_image = None
        self.poselog = open('poses.txt','w')
コード例 #22
0
ファイル: dashboard.py プロジェクト: uf-mil/software-common
 def __init__(self, ns, ActionSpec):
     ActionClient.__init__(self, ns, ActionSpec)
     self.manager = ExtendedGoalManager(ActionSpec)
     self.manager.register_send_goal_fn(self.pub_goal.publish)
     self.manager.register_cancel_fn(self.pub_cancel.publish)
     self.observe_goals = {}
     self.observer_transition_cb = None
     self.observer_feedback_cb = None
     self.goal_sub = rospy.Subscriber(rospy.remap_name(ns) + '/goal', self.ActionGoal,
                                      callback=self.goal_cb, queue_size=5)
コード例 #23
0
    def __init__(self, size, dim):

        self.chess_size = size
        self.dim = dim

        left_ns = rospy.remap_name('left')
        range_ns = rospy.remap_name('range')
        limg_sub = message_filters.Subscriber(left_ns + '/image_rect', sensor_msgs.msg.Image)
        rimg_sub = message_filters.Subscriber(range_ns + '/image_rect', sensor_msgs.msg.Image)
        linfo_sub = message_filters.Subscriber(left_ns + '/camera_info',sensor_msgs.msg.CameraInfo)
        rinfo_sub = message_filters.Subscriber(range_ns + '/camera_info', sensor_msgs.msg.CameraInfo)

        ts = message_filters.TimeSynchronizer([limg_sub, linfo_sub, rimg_sub, rinfo_sub], 16)
        ts.registerCallback(self.queue_cr)
        self.bridge = cv_bridge.CvBridge()

        self.frame_list = []
        self.static_pose = None
        self.result_list = []
        self.last_err = 0
コード例 #24
0
ファイル: dashboard.py プロジェクト: thefata/mil_common
 def __init__(self, ns, ActionSpec):
     ActionClient.__init__(self, ns, ActionSpec)
     self.manager = ExtendedGoalManager(ActionSpec)
     self.manager.register_send_goal_fn(self.pub_goal.publish)
     self.manager.register_cancel_fn(self.pub_cancel.publish)
     self.observe_goals = {}
     self.observer_transition_cb = None
     self.observer_feedback_cb = None
     self.goal_sub = rospy.Subscriber(rospy.remap_name(ns) + '/goal',
                                      self.ActionGoal,
                                      callback=self.goal_cb,
                                      queue_size=5)
コード例 #25
0
    def __init__(self, name):
        self._lock = threading.Lock()
        self._action_name = name
        self._board = None

        self._imageSub = rospy.Subscriber("image_raw", Image,
                                          self.img_callback)
        rospy.loginfo("[%s] Subscribing to: %s" %
                      (rospy.get_name(), rospy.remap_name("image_raw")))
        self._featurePub = rospy.Publisher("features", CalibrationPattern)
        self._calImagePub = rospy.Publisher("calibration_image", Image)
        rospy.loginfo("[%s] Publishing features to: %s" %
                      (rospy.get_name(), rospy.remap_name("features")))
        rospy.loginfo(
            "[%s] Publishing calibration_image to: %s" %
            (rospy.get_name(), rospy.remap_name("calibration_image")))

        self._server = actionlib.SimpleActionServer(self._action_name,
                                                    ConfigAction, None, False)
        self._server.register_goal_callback(self.goal_callback)
        self._server.register_preempt_callback(self.preempt_callback)

        self._server.start()
コード例 #26
0
    def __init__(self,
                 boards,
                 service_check=True,
                 synchronizer=message_filters.TimeSynchronizer,
                 flags=0):
        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

        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.q_mono = Queue.Queue()

        self.c = None

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()
コード例 #27
0
    def __init__(self, action_namespace, action_spec):
        self.goals = []
        self.client = ActionClient(action_namespace, action_spec)
        self.status_topic = rospy.remap_name(action_namespace) + '/status'
        self.status_sub = rospy.Subscriber(self.status_topic, GoalStatusArray,
                                           self.add_missing_goals)
        # self.status_sub = rospy.Subscriber(
        #     rospy.remap_name(action_namespace) + '/goal',
        #     self.client.ActionGoal,
        #     self.add_new_goal)

        self.client.wait_for_server()
        rospy.wait_for_service(action_namespace + '/get_goal_from_id')
        self.goal_from_id = rospy.ServiceProxy(
            action_namespace + '/get_goal_from_id', GetTaskFromID)
コード例 #28
0
ファイル: action_client.py プロジェクト: jdddog/hri
    def __init__(self, ns, ActionSpec):
        self.ns = ns
        self.last_status_msg = None

        try:
            a = ActionSpec()

            self.ActionSpec = ActionSpec
            self.ActionGoal = type(a.action_goal)
            self.ActionResult = type(a.action_result)
            self.ActionFeedback = type(a.action_feedback)
        except AttributeError:
            raise ActionException("Type is not an action spec: %s" % str(ActionSpec))

        self.pub_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal', self.ActionGoal, queue_size=100)
        self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel', GoalID, queue_size=100)

        self.manager = GoalManager(ActionSpec)
        self.manager.register_send_goal_fn(self.pub_goal.publish)
        self.manager.register_cancel_fn(self.pub_cancel.publish)

        self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, self._status_cb)
        self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, self._result_cb)
        self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, self._feedback_cb)
コード例 #29
0
    def handle_get_calibration_srv(self, req):
        """
        Handles requests for the camera calibration parameters.
        """
        with self.lock:
            if self.c.calibrated:
                ost_txt = self.c.ost()
            else:
                ost_txt = ''

        if ost_txt:
            # Add actuall camera name to calibration
            camera_topic = rospy.remap_name('camera')
            camera_name = camera_topic.split('/')[2]
            ost_txt = ost_txt.replace('narrow_stereo/left', camera_name)

        return GetStringResponse(ost_txt)
コード例 #30
0
    def __init__(self, action_namespace, action_spec):
        self.client = SimpleActionClient(action_namespace, action_spec)
        self.status_topic = rospy.remap_name(action_namespace) + '/status'
        # self.status_sub = rospy.Subscriber(
        #     self.status_topic,
        #     GoalStatusArray,
        #     self.add_missing_goal)
        # self.status_sub = rospy.Subscriber(
        #     rospy.remap_name(action_namespace) + '/goal',
        #     self.client.action_client.ActionGoal,
        #     self.add_new_goal)

        rospy.logdebug('Connecting to action server')
        self.client.wait_for_server()
        rospy.logdebug('Waiting for sync service')
        rospy.wait_for_service(action_namespace + '/get_goal_from_id')
        self.goal_from_id = rospy.ServiceProxy(
            action_namespace + '/get_goal_from_id', GetTaskFromID)
コード例 #31
0
ファイル: roam_node2.py プロジェクト: PR2/linux_networking
 def __init__(self):
     Node("multi_interface_roam")
     self.interface_selector = interface_selector.InterfaceSelector()
     self.reconfig_server = dynamic_reconfigure.server.Server(MultiInterfaceRoamConfig, self.reconfigure)
     self._interfaces = self.interface_selector.interfaces.values()
     self.hostname = os.uname()[1]
     
     # Prepare topics to publish
     pub_namespace = rospy.remap_name('wifi')
     self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
     self.ap_pub = rospy.Publisher(pub_namespace+"/accesspoint", AccessPoint)
     self.status_pub = rospy.Publisher(pub_namespace+"/status", MultiInterfaceStatus)
     self.iface_id_pub = rospy.Publisher(pub_namespace+'/current_iface_id', Int32, latch = True)
     self._wireless_interfaces = [ i for i in self._interfaces if i.__class__ == interface.WirelessInterface ]
     self.all_ap_pub = dict((iface, rospy.Publisher(pub_namespace+"/"+iface.iface+"/accesspoint", AccessPoint)) for iface in self._wireless_interfaces)
     
     # Kick off publication updates.
     self.interface_selector.update_event.subscribe_repeating(self._publish_status)
コード例 #32
0
def main():
    topic_name = 'chatter'

    pub = rospy.Publisher(topic_name, Dog, queue_size=10)
    rospy.init_node('publisher', anonymous=True)
    frequency = rospy.get_param('~freq', 1)

    rate = rospy.Rate(frequency)

    dog_msg = Dog()
    dog_msg.name = 'Bobi'
    dog_msg.age = 7
    dog_msg.color = 'Black'
    dog_msg.brothers.append('Lassie')

    while not rospy.is_shutdown():
        rospy.loginfo('Publishing messege: ' + Fore.RED + str(dog_msg) +
                      Fore.RESET + '" on topic ' +
                      rospy.remap_name(topic_name))
        pub.publish(dog_msg)
        rate.sleep()
コード例 #33
0
ファイル: talker.py プロジェクト: samuelf98/PARI_20-21_sf
def talker():

    Nome_topico = 'Conversa'
    frequency = 10

    #setup do publicador: as mensagens já n ão são do tipo string, mas do Num (mensagem)
    pub = rospy.Publisher(Nome_topico, Mensagem_Dog, queue_size=10)
    rospy.init_node(
        'publisher', anonymous=True
    )  #Posso colocar o anonymous a false, mas depois pode haver colisões de canais. O publisher pode mudar para Samuel
    rate = rospy.Rate(frequency)

    while not rospy.is_shutdown():
        hello_str = "Publicar: " + Fore.RED + str(
            dog_msg) + Fore.RESET + " on topic: " + rospy.remap_name(
                Nome_topico)  #Altera a string de debug o nome do topico
        rospy.loginfo(hello_str)
        pub.publish(
            dog_msg
        )  #Aqui tenho de enviar a minha mensagem do Num que foi criada à parte.
        rate.sleep()
コード例 #34
0
def talker():

    Nome_topico = 'Conversa'
    frequency = 10

    pub = rospy.Publisher(Nome_topico, Mensagem_Dog, queue_size=10)
    rospy.init_node('publisher', anonymous=False)
    rate = rospy.Rate(frequency)

    while not rospy.is_shutdown():
        # Get value of parameter (first i had to make de set of the parame.)
        cor = rospy.get_param(
            'highlight_text_color'
        )  #Como estou a usar um parametro privado( que gravei com o nome publisher) meto o til e assim reconhece esse nome
        hello_str = "Publicar: " + getattr(Fore, cor) + str(
            dog_msg) + Fore.RESET + " on topic: " + rospy.remap_name(
                Nome_topico)  #Altera a string de debug o nome do topico
        rospy.loginfo(hello_str)
        pub.publish(
            dog_msg
        )  #Aqui tenho de enviar a minha mensagem do Num que foi criada à parte.
        rate.sleep()
コード例 #35
0
 def __init__(self):
     self.val = None
     sub = rospy.Subscriber(rospy.remap_name('info'), sensor_msgs.msg.CameraInfo, self.setcam)
コード例 #36
0
 def scoped_name(self, name):
     return rosgraph.names.canonicalize_name('/%s/%s'%(self._robot_name, rospy.remap_name(name)))
コード例 #37
0
    def __init__(self,
                 boards,
                 camera_namespace,
                 image_topic,
                 synchronizer=message_filters.TimeSynchronizer,
                 pattern=Patterns.Chessboard,
                 directory='/tmp',
                 min_samples=0):

        fullservicename = "%s/set_camera_info" % camera_namespace
        print "Waiting for service %s ..." % (fullservicename)
        try:
            rospy.wait_for_service(fullservicename, 5)
            print("OK")
        except rospy.ROSException:
            print("Service not found")
            rospy.signal_shutdown('Quit')

        #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 %s ..." %(fullservicename)
        #try:
        #rospy.wait_for_service(fullservicename, 5)
        #print("OK")
        #except rospy.ROSException:
        #print("Service not found")
        #rospy.signal_shutdown('Quit')

        # Minimum number of samples to capture
        if min_samples <= 0:
            self.min_samples_number = float('inf')
        else:
            self.min_samples_number = min_samples
        self._boards = boards
        self._calib_flags = 0
        self._pattern = pattern
        self._camera_name = ''  # Delete?
        self._directory = directory
        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_topic, sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)

        self.set_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % camera_namespace,
            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 = Queue()
        self.q_stereo = Queue()

        self.c = None
        self.captured_images_count = 0
        self.capture_img_permission = False
        self.image_captured = False
        self.image_captured_server()

        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.enough_samples = False
        self.check_goodenough()

        rospy.signal_shutdown('Quit')
コード例 #38
0
                              (rospy.get_name(), self._file_name))
                writeCalibration(self._file_name, self._camera_name,
                                 result_msg.result)

            rospy.loginfo("[%s] Finished Calibrating" % rospy.get_name())
            self._calibrator = None


if __name__ == '__main__':
    rospy.init_node("mono_calibrator")
    rospy.loginfo("[%s] Starting.." % rospy.get_name())
    params = CalibratorParams()
    use_service = rospy.get_param("~use_service", True)
    autostart = rospy.get_param("~autostart", False)

    params.features = rospy.remap_name("features")
    params.camera_info = rospy.remap_name("camera_info")
    params.service_name = "%s/set_camera_info" % rospy.remap_name("camera")
    params.camera_name = rospy.get_param("~camera_name", "camera")
    params.file_name = rospy.get_param("~file_name", "camera.yaml")

    # Show user the parameters used
    rospy.loginfo("[" + rospy.get_name() + "] ~autostart:=" + str(autostart))
    rospy.loginfo("[" + rospy.get_name() + "] ~use_service:=" +
                  str(use_service))
    rospy.loginfo("[" + rospy.get_name() + "] features:=" +
                  rospy.resolve_name(params.features))
    rospy.loginfo("[" + rospy.get_name() + "] camera_info:=" +
                  rospy.resolve_name(params.camera_info))
    rospy.loginfo("[" + rospy.get_name() + "] set_camera_info:=" +
                  rospy.resolve_name(params.service_name))
コード例 #39
0
    rfill(m.R)
    rfill(m.P)
    return m

class CamInfoTracker:
    def __init__(self):
        self.val = None
        sub = rospy.Subscriber(rospy.remap_name('info'), sensor_msgs.msg.CameraInfo, self.setcam)
    def setcam(self, caminfo):
        self.val = caminfo
        self.age = time.time()

rospy.init_node('camera_hammer')
track = CamInfoTracker()

service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)
for i in range(1000):
    print("\nIteration", i)
    m = random_camerainfo()
    print(m)
    response = service(m)
    print(response)
    start = rospy.get_time()
    outcome = False
    while True:
        try:
            outcome = list(track.val.P) == list(m.P)
        except:
            pass
        if outcome:
            break
コード例 #40
0
        _msg.pose.pose.position.z = position[2]
        _msg.twist.twist.linear.x = data.twist.twist.linear.x
        _msg.twist.twist.angular.z = data.twist.twist.linear.x
        pub.publish(_msg)
    else:
        rospy.logwarn('Frame does not exist : %s, %s', _base_frame_name,
                      _map_frame_name)


if __name__ == '__main__':
    rospy.init_node('odompublisher', anonymous=False)
    rospy.loginfo('Starting odometry publishing node')
    # get parameters
    _base_frame_name = rospy.get_param('~base_frame', default='base_link')
    _map_frame_name = rospy.get_param('~map_frame', default='map')
    # remap parameters
    _odom_name = rospy.remap_name('odom')
    _odom_repub_name = rospy.remap_name('odom2')
    # Publishers
    pub = rospy.Publisher(_odom_repub_name, Odometry, queue_size=10)
    rospy.loginfo('Publishing odometry to %s', _odom_repub_name)
    # Initialize Listener
    tlisten = TransformListener()
    # Subscirbers
    rospy.Subscriber(_odom_name, Odometry, callback_odom)
    rospy.loginfo('Subscribed to %s', _odom_name)
    # spin
    rospy.spin()
    # shutdown
    rospy.loginfo('Shutting Down')
コード例 #41
0
 def warn_remap(self, name):
     if name == rospy.remap_name(name):
         rospy.logwarn("topic '%s' is not remapped" % name)
コード例 #42
0
def cal_from_tarfile(boards,
                     tarname,
                     mono=False,
                     upload=False,
                     calib_flags=0,
                     visualize=False,
                     alpha=1.0):
    if mono:
        calibrator = MonoCalibrator(boards, calib_flags)
    else:
        calibrator = StereoCalibrator(boards, calib_flags)

    calibrator.do_tarfile_calibration(tarname)

    print(calibrator.ost())

    if upload:
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy(
                "%s/set_camera_info" % rospy.remap_name("camera"),
                sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError(
                    "connected to set_camera_info service, but failed setting camera_info"
                )
        else:
            set_left_camera_info_service = rospy.ServiceProxy(
                "%s/set_camera_info" % rospy.remap_name("left_camera"),
                sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy(
                "%s/set_camera_info" % rospy.remap_name("right_camera"),
                sensor_msgs.srv.SetCameraInfo)

            response1 = set_left_camera_info_service(info[0])
            response2 = set_right_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError(
                    "connected to set_camera_info service, but failed setting camera_info"
                )

    if visualize:

        #Show rectified images
        calibrator.set_alpha(alpha)

        archive = tarfile.open(tarname, 'r')
        if mono:
            for f in archive.getnames():
                if f.startswith('left') and (f.endswith('.pgm')
                                             or f.endswith('png')):
                    filedata = archive.extractfile(f).read()
                    file_bytes = numpy.asarray(bytearray(filedata),
                                               dtype=numpy.uint8)
                    im = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)

                    bridge = cv_bridge.CvBridge()
                    try:
                        msg = bridge.cv2_to_imgmsg(im, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #handle msg returns the recitifed image with corner detection once camera is calibrated.
                    drawable = calibrator.handle_msg(msg)
                    vis = numpy.asarray(drawable.scrib[:, :])
                    #Display. Name of window:f
                    display(f, vis)
        else:
            limages = [
                f for f in archive.getnames() if (f.startswith('left') and (
                    f.endswith('pgm') or f.endswith('png')))
            ]
            limages.sort()
            rimages = [
                f for f in archive.getnames() if (f.startswith('right') and (
                    f.endswith('pgm') or f.endswith('png')))
            ]
            rimages.sort()

            if not len(limages) == len(rimages):
                raise RuntimeError(
                    "Left, right images don't match. %d left images, %d right"
                    % (len(limages), len(rimages)))

            for i in range(len(limages)):
                l = limages[i]
                r = rimages[i]

                if l.startswith('left') and (
                        l.endswith('.pgm')
                        or l.endswith('png')) and r.startswith('right') and (
                            r.endswith('.pgm') or r.endswith('png')):
                    # LEFT IMAGE
                    filedata = archive.extractfile(l).read()
                    file_bytes = numpy.asarray(bytearray(filedata),
                                               dtype=numpy.uint8)
                    im_left = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)

                    bridge = cv_bridge.CvBridge()
                    try:
                        msg_left = bridge.cv2_to_imgmsg(im_left, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #RIGHT IMAGE
                    filedata = archive.extractfile(r).read()
                    file_bytes = numpy.asarray(bytearray(filedata),
                                               dtype=numpy.uint8)
                    im_right = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
                    try:
                        msg_right = bridge.cv2_to_imgmsg(im_right, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    drawable = calibrator.handle_msg([msg_left, msg_right])

                    h, w = numpy.asarray(drawable.lscrib[:, :]).shape[:2]
                    vis = numpy.zeros((h, w * 2, 3), numpy.uint8)
                    vis[:h, :w, :] = numpy.asarray(drawable.lscrib[:, :])
                    vis[:h, w:w * 2, :] = numpy.asarray(drawable.rscrib[:, :])

                    display(l + " " + r, vis)
コード例 #43
0
def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0):
    if mono:
        calibrator = MonoCalibrator(boards, calib_flags)
    else:
        calibrator = StereoCalibrator(boards, calib_flags)

    calibrator.do_tarfile_calibration(tarname)

    print(calibrator.ost())

    if upload: 
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
        else:
            set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)

            response1 = set_left_camera_info_service(info[0])
            response2 = set_right_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")

    if visualize:

        #Show rectified images
        calibrator.set_alpha(alpha)

        archive = tarfile.open(tarname, 'r')
        if mono:
            for f in archive.getnames():
                if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')):
                    filedata = archive.extractfile(f).read()
                    file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
                    im=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)

                    bridge = cv_bridge.CvBridge()
                    try:
                        msg=bridge.cv2_to_imgmsg(im, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #handle msg returns the recitifed image with corner detection once camera is calibrated.
                    drawable=calibrator.handle_msg(msg)
                    vis=numpy.asarray( drawable.scrib[:,:])
                    #Display. Name of window:f
                    display(f, vis)
        else:
            limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ]
            limages.sort()
            rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ]
            rimages.sort()

            if not len(limages) == len(rimages):
                raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages)))
            
            for i in range(len(limages)):
                l=limages[i]
                r=rimages[i]

                if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')):
                    # LEFT IMAGE
                    filedata = archive.extractfile(l).read()
                    file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
                    im_left=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
       
                    bridge = cv_bridge.CvBridge()
                    try:
                        msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #RIGHT IMAGE
                    filedata = archive.extractfile(r).read()
                    file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
                    im_right=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
                    try:
                        msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    drawable=calibrator.handle_msg([ msg_left,msg_right] )

                    h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2]
                    vis = numpy.zeros((h, w*2, 3), numpy.uint8)
                    vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:])
                    vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:])
                    
                    display(l+" "+r,vis)    
コード例 #44
0
 def scoped_name(self, name):
     return rosgraph.names.canonicalize_name(
         '/%s/%s' % (self._robot_name, rospy.remap_name(name)))
コード例 #45
0
 def scoped_name(self, name):
     return roslib.names.canonicalize_name("/%s/%s" % (self._robot_name, rospy.remap_name(name)))
コード例 #46
0
def check_remapped(name):
    if name == rospy.remap_name(name):
        rospy.logerr("Topic %s was not remapped. This is probably unintentional."%name)
コード例 #47
0
 def __init__(self):
     self.val = None
     sub = rospy.Subscriber(rospy.remap_name('info'), sensor_msgs.msg.CameraInfo, self.setcam)