def main():
pub = rospy.TopicPub('image', Image)
rospy.ready('test_send')
image = cv.cvCreateImage(cv.cvSize(640, 480), 8, 1)
cv.cvSet(image, 133)
while not rospy.is_shutdown():
pub.publish(
Image(None, 'test', image.width, image.height, 'none', 'mono8',
image.imageData))
time.sleep(.033)
rospy.spin()
def test_arm(self):
pub = rospy.TopicPub("initialpose", Pose2DFloat32)
rospy.ready(NAME, anonymous=True)
time.sleep(10.0)
start = Pose2DFloat32()
start.x = 0
start.y = 0
start.th = 0
pub.publish(start)
timeout_t = time.time() + 110.0 #90 seconds
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
self.test.readLogFile()
self.success = self.test.getPassed()
time.sleep(0.2)
os.system("killall trex_fast")
os.system("killall gazebo")
self.assert_(self.success)
def advertiseInitalPose2d(self, name):
self.pub = rospy.TopicPub("initialpose", Pose2DFloat32)
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 listener():
pub = rospy.TopicPub("/diagnostics", DiagnosticMessage)
rospy.ready(NAME, anonymous=True)
while not rospy.is_shutdown():
loop(pub)
time.sleep(1)
def start_publishing():
global _pub
if _pub is not None:
return
print "registering onto %s" % OUT
_pub = rospy.TopicPub(OUT, MSG)
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # ## @author Hai Nguyen/[email protected] ##@package image_broadcaster # Broadcast image given on command line to topic 'image'. import rostools rostools.updatePath('gmmseg') import rospy import std_msgs.msg.Image as RImage import opencv.highgui as hg import opencv as cv import sys import time rospy.ready(sys.argv[0]) pub_channel = rospy.TopicPub('image', RImage) cv_image = hg.cvLoadImage(sys.argv[1]) cv.cvCvtColor(cv_image, cv_image, cv.CV_BGR2RGB) count = 1 while not rospy.isShutdown(): image = RImage(None, 'static', cv_image.width, cv_image.height, '', 'rgb24', cv_image.imageData) pub_channel.publish(image) print 'published', count count = count + 1 time.sleep(.3)
PKG = 'laser_interface'
import sys, os, subprocess
try:
rostoolsDir = (subprocess.Popen(['rospack', 'find', 'rostools'],
stdout=subprocess.PIPE).communicate()[0]
or '').strip()
sys.path.append(os.path.join(rostoolsDir, 'src'))
import rostools.launcher
rostools.launcher.updateSysPath(sys.argv[0], PKG, bootstrapVersion="0.6")
except ImportError:
print >> sys.stderr, "\nERROR: Cannot locate rostools"
sys.exit(1)
#from pkg import *
import rospy
from std_msgs.msg import Position
from std_msgs.msg import RobotBase2DOdom
from std_msgs.msg import Pose2DFloat32
import sys
import time
pub = rospy.TopicPub('odom', RobotBase2DOdom)
rospy.ready(sys.argv[0])
while not rospy.isShutdown():
odom = RobotBase2DOdom(None, Pose2DFloat32(1, 2, 3),
Pose2DFloat32(1, 2, 4), 1)
pub.publish(odom)
time.sleep(.5)
print 'published'