def __init__(self, decimate):
rospy.TopicSub('/videre/images', ImageArray, self.handle_array, queue_size=2, buff_size=7000000)
rospy.TopicSub('/videre/cal_params', String, self.handle_params)
self.pub_vo = rospy.Publisher("/vo", VOPose)
self.vo = None
self.decimate = decimate
self.modulo = 0
def main():
vod = VODemo()
if SEE:
hg.cvNamedWindow('channel L', 1)
hg.cvNamedWindow('channel R', 1)
rospy.TopicSub('/videre/images', ImageArray, vod.display_array)
rospy.TopicSub('/videre/cal_params', String, vod.display_params)
rospy.TopicSub('image', Image, display_single)
rospy.ready('camview_py')
rospy.spin()
def main(argv):
people_tracker = PeopleTracker()
if len(argv) > 0 and argv[0] == "-bag":
people_tracker.usebag = True
# Use a bag file
if people_tracker.usebag:
import rosrecord
#filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag"
filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag"
#filename = "/wg/stor2/prdata/videre-bags/face2.bag"
if SAVE_PICS:
try:
os.mkdir("/tmp/tiff/")
except:
pass
num_frames = 0
start_frame = 4700
end_frame = 4900
for topic, msg in rosrecord.logplayer(filename):
if topic == '/videre/cal_params':
people_tracker.params(msg)
elif topic == '/videre/images':
if num_frames >= start_frame and num_frames < end_frame:
people_tracker.frame(msg)
num_frames += 1
else:
pass
# Use new ROS messages, and output the 3D position of the face in the camera frame.
else:
print "Non-bag"
if SAVE_PICS:
try:
os.mkdir("/tmp/tiff/")
except:
pass
people_tracker.pub = rospy.Publisher('head_controller/track_point',
PointStamped)
rospy.init_node('videre_face_tracker', anonymous=True)
rospy.TopicSub('/head_controller/track_point', PointStamped,
people_tracker.point_stamped)
rospy.TopicSub('/videre/images', ImageArray, people_tracker.frame)
rospy.TopicSub('/videre/cal_params', String, people_tracker.params)
rospy.spin()
def __init__(self, mode, library):
self.mode = mode
self.library = library
rospy.TopicSub('/videre/images', ImageArray, self.display_array)
rospy.TopicSub('/videre/cal_params', String, self.display_params)
rospy.TopicSub('/vo/tmo', Pose44, self.handle_corrections)
self.viz_pub = rospy.Publisher("/overlay", Lines)
self.vo_key_pub = rospy.Publisher("/vo/key", Frame)
self.Tmo = Pose()
self.mymarker1 = Marker(10)
self.mymarkertrail = [Marker(11 + i) for i in range(10)]
self.trail = []
self.vis = Vis()
def test_scan(self):
print "LNK\n"
rospy.TopicSub("base_scan", LaserScan, self.pointInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(self.success)
def run_me_server():
sn = SegmentNode()
hg.cvNamedWindow('image', 1)
rospy.TopicSub('image', RImage, sn.set_image)
rospy.ready(sys.argv[0])
segment_service = rospy.Service('hrl_grasp', srv.hrl_grasp, sn.segment)
segment_service.register()
rospy.spin()
def test_pointcloud(self):
print "LNK\n"
rospy.TopicSub("base_scan", LaserScan, self.pointInput)
rospy.ready(NAME, anonymous=True)
timeout_t = time.time() + 5.0
while not rospy.is_shutdown(
) and not self.success and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(self.success)
def __init__(self, vo, library):
self.vo = vo
self.library = library
rospy.TopicSub('/vo/key', Frame, self.incoming_frame)
self.pub_tmo = rospy.Publisher("/vo/tmo", Pose44)
self.frameq = Queue()
self.mark0 = Marker(1)
self.mark1 = Marker(2)
def OnInit(self):
rospy.ready("TestPlotter", anonymous=True)
self.data_topic = rospy.TopicSub("/test_data", TestData, self.OnData)
# Configure the plot panel
self.plot = wxmpl.PlotApp('Test Plot')
self.plot.set_crosshairs(True)
self.plot.set_selection(True)
self.plot.set_zoom(True)
return True
def testcameras(self):
print " wait 3 sec for objects to settle "
time.sleep(3)
print " subscribe image from ROS "
rospy.TopicSub("test_camera/image", Image, self.imageInput)
rospy.init_node(NAME, anonymous=True)
#self.pollThread.start()
timeout_t = time.time() + 10 #10 seconds delay for processing comparison
while not rospy.is_shutdown() and not self.tested and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(self.success)
def OnInit(self):
rospy.ready("Hysteresis", anonymous=True)
self.data_dict = {}
self.data_topic = rospy.TopicSub("/hysteresis_data", ChannelFloat32, self.OnData)
# Configure the plot panel
self.plot = wxmpl.PlotApp('Hysteresis Plot')
self.plot.set_crosshairs(False)
self.plot.set_selection(False)
self.plot.set_zoom(False)
return True
def test_cloud(self):
print "LNK\n"
rospy.TopicSub("world_3d_map", PointCloudFloat32, self.pointInput)
rospy.ready(NAME, anonymous=True)
timeout_t = time.time() + 30.0 #30 seconds
while not rospy.is_shutdown(
) and not self.success and time.time() < timeout_t:
time.sleep(0.1)
self.success = self.errorcheck
os.system("killall gazebo")
self.assert_(self.success)
def testcameras(self):
print " wait TEST_INIT_WAIT sec for objects to settle "
time.sleep(TEST_INIT_WAIT)
print " subscribe stereo left image from ROS "
#rospy.TopicSub("test_camera/image", Image, self.imageInput) # this is a test camera, simply looking at the cups
rospy.TopicSub("stereo_l/image", Image, self.imageInput) # this is a camera mounted on PR2 head (left stereo camera)
rospy.init_node(NAME, anonymous=True)
#self.pollThread.start()
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(self.success)
def testslide(self):
print "LINK\n"
#rospy.TopicSub("Odom", RobotBase2DOdom, self.positionInput)
rospy.TopicSub("base_pose_ground_truth", TransformWithRateStamped,
self.positionInput)
rospy.ready(NAME, anonymous=True)
timeout_t = time.time() + 50.0 #59 seconds
while not rospy.is_shutdown(
) and not self.success and not self.fail and time.time() < timeout_t:
time.sleep(0.1)
time.sleep(2.0)
#os.system("killall gazebo")
self.assert_(self.success)
def test_wpc(self):
rospy.TopicSub("groundtruthposition", Point32, self.positionInput)
self.test.advertiseInitalPose2d(NAME)
rospy.ready(NAME, anonymous=True)
time.sleep(2.0)
self.test.publishInitalPose2d(NAME)
timeout_t = time.time() + 50.0 #59 seconds
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
time.sleep(0.1)
time.sleep(2.0)
os.system("killall trex_fast")
self.assert_(self.success)
def subscribe(self, itemPath, style, ChannelType=RosChannel):
item = self._getItem(itemPath)
topicName = self._getTopic(itemPath)
assert topicName in self.topics
if not self.topics[topicName].isRegistered:
self.topicsSubs[topicName] = RosSubscriber(self.topics[topicName])
messageName, messageModName, messageType = self.topics[topicName].qualifiedMessageType
exec('import %s' % messageModName)
rospy.TopicSub(self._getTopic(itemPath), eval(messageType), self.topicsSubs[topicName].callback)
channel = ChannelType(itemPath, style)
item.channels.append(channel)
return channel
def OnInit(self):
rospy.ready("SineSweep", anonymous=True)
self.data_dict = {}
self.count = 0
self.data_topic = rospy.TopicSub("/sinesweep_data", ChannelFloat32,
self.OnData)
# Configure the plot panel
self.plot = wxmpl.PlotApp('Sine Sweep Plot')
self.plot.set_crosshairs(True)
self.plot.set_selection(True)
self.plot.set_zoom(True)
return True
def registerTopic(self, topicName, topicTypeName, callback):
"""Register to the topic using only the names (strings) of the topic, type."""
# TODO: encapsulate with exceptions
# Loading the packageName
# TODO: how to check if it is already loaded?
print 'topic type: %s' % topicTypeName
(modName, messageName) = messageNames(topicTypeName)
importMessageModule(modName, messageName)
exec('import %s' % messageName)
print eval(messageName)
rospy.TopicSub(topicName, eval(messageName), callback)
print 'registering done'
def test_ms_msg(self):
self.assertEquals(0, len(self.callback_data), "invalid test fixture")
if 0:
# wait at most 5 seconds for publisher to be registered
timeout_t = time.time() + 5.0
while not rostest.is_subscriber(
rospy.resolve_name(PUBTOPIC),
rospy.resolve_name(PUBNODE)) and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(
rostest.is_subscriber(rospy.resolve_name(PUBTOPIC),
rospy.resolve_name(PUBNODE)),
"%s is not up" % PUBNODE)
print "Subscribing to ", SUBTOPIC
rospy.TopicSub(SUBTOPIC, MSG, self._test_ms_callback)
sleep_time = 3.0
print "Waiting %s seconds to collect messages" % sleep_time
time.sleep(sleep_time)
# make sure we got it
self.assert_(len(self.callback_data),
"no callback data from %s" % PUBNODE)
for d in self.callback_data:
errorstr = "callback msg field [%%s] from %s does not match" % PUBNODE
self.assertAlmostEqual(3.14, d.time, 2, errorstr % "time")
self.assertEquals(250, len(d.joint_states))
self.assertEquals(260, len(d.actuator_states))
for f in d.joint_states:
self.assertEquals("jointstate", f.name)
self.assertAlmostEqual(1.0, f.position, 1)
self.assertAlmostEqual(1.0, f.velocity, 1)
self.assertAlmostEqual(1.0, f.applied_effort, 1)
self.assertAlmostEqual(1.0, f.commanded_effort, 1)
for i in xrange(0, len(d.actuator_states)):
f = d.actuator_states[i]
self.assertEquals("actuatorstate", f.name)
self.assertEquals(1.0, f.motor_voltage)
self.assertEquals(i, f.encoder_count)
self.assertEquals(0, f.calibration_reading)
self.assertEquals(i + 1, f.last_calibration_low_transition)
self.assertEquals(i + 2, f.last_calibration_high_transition)
self.assertEquals(0, f.num_encoder_errors)
self.addPoint(state.PosAct)
# A worker class for the ros node
# To be deprecated soon
class WorkerListener(Thread):
def __init__(self):
Thread.__init__(self)
def run(self):
rospy.ready(NAME, anonymous=True)
rospy.spin()
channel = MyChannel()
channel2 = MyChannelAct()
rospy.TopicSub('ARM_L_PAN_state', RotaryJointState, channel.callback)
rospy.TopicSub('ARM_L_PAN_state', RotaryJointState, channel2.callback)
app = wx.PySimpleApp(0)
worker = WorkerListener()
worker.start()
frame = wx.Frame(None, -1, "")
panel = MyPlot(frame)
panel.addChannel(channel)
panel.addChannel(channel2)
frame.Show()
print '>>>>>>>>>>>>>>'
app.MainLoop()
print '>>>>>>>>>>>>>>'
#rospy.spin()
class ForearmTest(BaseTest):
def __init__(self, parent, serial, func):
self.ready = False
self.serial1 = serial[0:7]
print self.serial1
self.count = 1
self.finished = False
self.testcount = 1
# Load the XRC resource
self.res2 = xrc.XmlResource(execution_path('forearm_test.xrc'))
# Initialize the BaseTest with these parts
BaseTest.__init__(self, parent, serial, func, 'forearm Test')
self.roslaunch = None
self.topic = None
self.data_topic = None
self.response = DiagnosticMessage(None, [])
self.data_dict = {}
def instruct_panel_gen(self, parent):
# Load the instruction and test panels from the XRC resource
panel = 'instruct_panel_' + self.serial1
print panel
return self.res2.LoadPanel(parent, panel)
def test_panel_gen(self, parent):
test_panel = self.res2.LoadPanel(parent, 'test_panel')
test_panel.Bind(wx.EVT_BUTTON,
self.EStop,
id=xrc.XRCID('ESTOP_button'))
return self.res2.LoadPanel(parent, 'test_panel')
# This is what runs once the instructions are read
def Run(self):
thread.start_new_thread(self.RunThread, (None, ))
def RunThread(self, arg):
#Create a roslauncher
config = roslaunch.ROSLaunchConfig()
self.count = 1
self.finished = False
self.testcount = 1
self.ready = False
# Try loading the test XML file
try:
xmlFile = execution_path(
str('xml/' + self.serial1 + '_forearm_test.xml'))
loader = roslaunch.XmlLoader()
loader.load(xmlFile, config)
except roslaunch.XmlParseException, e:
wx.CallAfter(
self.Cancel,
'Could not load back-end XML to launch necessary nodes. Make sure the GUI is up to date.'
)
return
# Make sure we get a fresh master
config.master.auto = config.master.AUTO_RESTART
# Bring up the nodes
self.roslaunch = roslaunch.ROSLaunchRunner(config)
self.roslaunch.launch()
self.topic = rospy.TopicSub("/diagnostics", DiagnosticMessage,
self.OnMsg)
self.data_topic = rospy.TopicSub("/hysteresis_data", ChannelFloat32,
self.OnData)
def listener_with_user_data():
rospy.TopicSub("/mechanism_state", MechanismState, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
def listener_with_user_data():
rospy.TopicSub("/pressure", PressureState, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
def listener_with_user_data():
rospy.TopicSub("/odom", RobotBase2DOdom, odom_callback)
rospy.TopicSub("/localizedpose", RobotBase2DOdom, localized_callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
def __init__(self):
rospy.init_node("TestPlotter", anonymous=True)
self.data_topic = rospy.TopicSub("/test_data", TestData, self.OnData)
import pyrob.util
#def callback_array(iar):
# left = iar.images[0]
# right = iar.images[1]
#
# start = time.time()
# leftcv = to_cv(left)
# rightcv = to_cv(right)
# print '%.4f' % (time.time() - start)
#
# print iar.header.seq,
# hg.cvShowImage('left', leftcv)
# hg.cvShowImage('right', rightcv)
# hg.cvWaitKey(5)
def callback_image(im):
t = time.time()
cvim = pyrob.util.ros2cv(im)
hg.cvShowImage('left', cvim)
hg.cvWaitKey(5)
print 'total', time.time() - t
if __name__ == '__main__':
hg.cvNamedWindow('left', 1)
rospy.TopicSub('image', Image, callback_image)
rospy.ready('test_pycv')
rospy.spin()
def listener_publisher():
rospy.ready(NAME)
rospy.TopicSub(IN, MSG, callback)
rospy.spin()
def test_ms_msg(self):
self.assert_(self.callback_data is None, "invalid test fixture")
# wait at most 5 seconds for listenerpublisher to be registered
timeout_t = time.time() + 5.0
while not rostest.is_subscriber(
rospy.resolve_name(PUBTOPIC),
rospy.resolve_name(LPNODE)) and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(
rostest.is_subscriber(rospy.resolve_name(PUBTOPIC),
rospy.resolve_name(LPNODE)),
"%s is not up" % LPNODE)
print "Publishing to ", PUBTOPIC
pub = rospy.TopicPub(PUBTOPIC, MSG)
print "Subscribing to ", SUBTOPIC
rospy.TopicSub(SUBTOPIC, MSG, self._test_ms_callback)
# publish about 10 messages for fun
import random
val = random.randint(0, 109812312)
msg = "hi [%s]" % val
header = None
for i in xrange(0, 10):
# The test message could be better in terms of the values
# it assigns to leaf fields, but the main focus is trying
# to dig up edge conditions in the embeds, especially with
# respect to arrays and embeds.
actuator_states = [
ActuatorState('name0', 0, 0.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0,
0.0, 0.0, 0.0, 0.0, 0),
ActuatorState('name1', 1, 1.0, 1.0, 1.0, 1.0, 1, 1, 1, 1, 1,
1.0, 1.0, 1.0, 1.0, 1),
]
joint_states = [
JointState('name0', 0.0, 0.0, 0.0, 0.0),
JointState('name1', 1.0, 1.0, 1.0, 1.0),
]
pub.publish(MSG(header, 2.0, actuator_states, joint_states))
time.sleep(0.1)
# listenerpublisher is supposed to repeat our messages back onto /listenerpublisher,
# make sure we got it
self.assert_(self.callback_data is not None,
"no callback data from listenerpublisher")
print "Got ", self.callback_data.time
errorstr = "callback msg field [%s] from listenerpublisher does not match"
self.assertEquals(2.0, self.callback_data.time, errorstr % "time")
self.assertEquals(2, len(self.callback_data.joint_states))
self.assertEquals(2, len(self.callback_data.actuator_states))
self.assertEquals("name0", self.callback_data.joint_states[0].name)
self.assertEquals("name0", self.callback_data.actuator_states[0].name)
self.assertEquals("name1", self.callback_data.joint_states[1].name)
self.assertEquals("name1", self.callback_data.actuator_states[1].name)
self.assertEquals(0,
self.callback_data.actuator_states[0].encoder_count)
self.assertEquals(
0, self.callback_data.actuator_states[0].calibration_reading)
self.assertEquals(
0,
self.callback_data.actuator_states[0].last_calibration_rising_edge)
self.assertEquals(
0, self.callback_data.actuator_states[0].num_encoder_errors)
self.assertEquals(1,
self.callback_data.actuator_states[1].encoder_count)
self.assertEquals(
1, self.callback_data.actuator_states[1].calibration_reading)
self.assertEquals(
1,
self.callback_data.actuator_states[1].last_calibration_rising_edge)
self.assertEquals(
1, self.callback_data.actuator_states[1].num_encoder_errors)
def main(argv):
people_tracker = PeopleTracker()
if len(argv) > 0 and argv[0] == "-bag":
people_tracker.usebag = True
# if len(argv)>1 and argv[1]=="-visualize":
# people_tracker.visualize = True
# Use a bag file
if people_tracker.usebag:
import rosrecord
#filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag"
filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag"
#filename = "/wg/stor2/prdata/videre-bags/face2.bag"
if SAVE_PICS:
try:
os.mkdir("/tmp/tiff/")
except:
pass
# if people_tracker.visualize:
# people_tracker.pub = rospy.Publisher('/std_msgs/full_cloud',PointCloud)
# rospy.init_node('videre_face_tracker',anonymous=True)
num_frames = 0
start_frame = 4700
end_frame = 5000
for topic, msg in rosrecord.logplayer(filename):
if topic == '/videre/cal_params':
people_tracker.params(msg)
elif topic == '/videre/images':
if num_frames >= start_frame and num_frames < end_frame:
people_tracker.frame(msg)
if people_tracker.visualize:
s = raw_input(
"Press any key in this window to proceed to the next frame."
)
elif num_frames >= end_frame:
break
num_frames += 1
else:
pass
# Use new ROS messages, and output the 3D position of the face in the camera frame.
else:
print "Non-bag"
if SAVE_PICS:
try:
os.mkdir("/tmp/tiff/")
except:
pass
#people_tracker.pub = rospy.Publisher('head_controller/track_point',PointStamped)
people_tracker.pub = rospy.Publisher(
'/stereo_face_feature_tracker/position_measurement',
PositionMeasurement)
rospy.init_node('videre_face_tracker', anonymous=True)
#rospy.TopicSub('/head_controller/track_point',PointStamped,people_tracker.point_stamped)
rospy.TopicSub('/videre/images', ImageArray, people_tracker.frame)
rospy.TopicSub('/videre/cal_params', String, people_tracker.params)
rospy.spin()
def listener_with_user_data():
rospy.TopicSub("/goal", Planner2DGoal, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
