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)
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")
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()
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")
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
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)
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()
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()
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)
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()
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
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)
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)
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
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)
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)
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()
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();
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();
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
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')
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)
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
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)
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()
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()
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)
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)
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)
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)
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)
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()
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()
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()
def __init__(self):
self.val = None
sub = rospy.Subscriber(rospy.remap_name('info'), sensor_msgs.msg.CameraInfo, self.setcam)
def scoped_name(self, name):
return rosgraph.names.canonicalize_name('/%s/%s'%(self._robot_name, rospy.remap_name(name)))
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')
(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))
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
_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')
def warn_remap(self, name):
if name == rospy.remap_name(name):
rospy.logwarn("topic '%s' is not remapped" % name)
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)
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)
def scoped_name(self, name):
return rosgraph.names.canonicalize_name(
'/%s/%s' % (self._robot_name, rospy.remap_name(name)))
def scoped_name(self, name):
return roslib.names.canonicalize_name("/%s/%s" % (self._robot_name, rospy.remap_name(name)))
def check_remapped(name):
if name == rospy.remap_name(name):
rospy.logerr("Topic %s was not remapped. This is probably unintentional."%name)
def __init__(self):
self.val = None
sub = rospy.Subscriber(rospy.remap_name('info'), sensor_msgs.msg.CameraInfo, self.setcam)
