Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

tf listenr test.

refs #1
  • Loading branch information...
commit cf40befa5e714c6cf0935e1f112e7e1d5f58c4a0 1 parent a2ebece
Takashi Ogura authored
2  rosruby_tf/lib/tf/broadcaster.rb
View
@@ -10,7 +10,7 @@ def initialize(node)
@publisher = node.advertise("/tf", TfMessage, :no_resolve=>true)
end
- def send_transform(translation, rotation, time, child, parent)
+ def send_transform(translation, rotation, time, parent, child)
ts_msg = Geometry_msgs::TransformStamped.new
ts_msg.header.frame_id = parent
ts_msg.header.stamp = time
13 rosruby_tf/lib/tf/listener.rb
View
@@ -11,17 +11,16 @@ def initialize(node)
tf_msg.transforms.each do |tf|
parent = @transform_buffer.find_transform(tf.header.frame_id,
tf.header.stamp)
-
- transform = Transform.new(tf.child_frame_id,
- [tf.transform.translation.x,
+ transform = Transform.new([tf.transform.translation.x,
tf.transform.translation.y,
tf.transform.translation.z],
[tf.transform.rotation.x,
tf.transform.rotation.y,
tf.transform.rotation.z,
tf.transform.rotation.w],
- parent)
- transform.stamp = tf.header.stamp
+ parent,
+ tf.child_frame_id,
+ tf.header.stamp)
@transform_buffer.add_transform(transform)
end
end
@@ -34,11 +33,9 @@ def initialize(node)
end
def lookup_transform(from_id, to_id, stamp=nil)
-
+
from = @transform_buffer.find_transform(from_id, stamp)
- p "from = #{from}"
to = @transform_buffer.find_transform(to_id, stamp)
- p "to = #{to}"
if from and to
from.get_transform_to(to)
else
66 rosruby_tf/lib/tf/transformer.rb
View
@@ -1,4 +1,4 @@
-#
+# tf/transformer.rb
#
#
require 'tf/quaternion'
@@ -14,6 +14,19 @@ module Tf
class Transform
+ # @param [Transform] parent
+ # @param [String] frame_id
+ # @param [ROS::Time] stamp
+ def initialize(pos=[0.0, 0.0, 0.0],
+ rot=[0.0, 0.0, 0.0, 1.0],
+ parent=nil, frame_id='', stamp=nil)
+ @frame_id = frame_id
+ @parent = parent
+ @pos = pos
+ @rot = rot
+ @stamp = stamp
+ end
+
def to_matrix
q = Quaternion.new(*@rot)
mat = q.to_matrix
@@ -23,23 +36,20 @@ def to_matrix
mat
end
- def initialize(frame_id, pos, rot, parent)
- @frame_id = frame_id
- @parent = parent
- @pos = pos
- @rot = rot
- @stamp = nil
- end
-
def to_s
- @frame_id
+ if @parent
+ "#{@frame_id} <= #{@parent.frame_id} : [#{@pos.join(",")}, #{@rot.join(",")}]"
+ else
+ "#{@frame_id} <= ROOT : [#{@pos.join(",")}, #{@rot.join(",")}]"
+ end
end
def get_path(target)
target_path = target.find_root
self_path = self.find_root
- if target_path.last == self_path.last
- while target_path.last == self_path.last
+ # same root
+ if target_path.last.frame_id == self_path.last.frame_id
+ while (not target_path.empty?) and (not self_path.empty?) and (target_path.last.frame_id == self_path.last.frame_id) do
root = target_path.last
target_path.pop
self_path.pop
@@ -66,15 +76,13 @@ def is_connected?(target)
def get_transform_to(target)
path = get_path(target)
- transform = Matrix::identity(4)
if path
+ transform = Matrix::identity(4)
for i in 0..(path.length-2)
- current_frame = path[i]
- next_frame = path[i+1]
- if current_frame.parent == next_frame
- transform *= current_frame.to_matrix.inverse
+ if path[i].parent == path[i+1]
+ transform *= path[i].to_matrix.inverse
else # this means next's parent is current
- transform *= next_frame.to_matrix
+ transform *= path[i+1].to_matrix
end
end
transform
@@ -92,24 +100,26 @@ def get_transform_to(target)
end
class TransformBuffer
- def initialize
- @max_buffer_length = 100
+
+ def initialize(max_buffer_length=100)
+ @max_buffer_length = max_buffer_length
@transforms = {}
end
+ attr_accessor :max_buffer_length
+
def find_transform(frame_id, stamp=nil)
- p "frame_id = #{frame_id}"
- p "stamp = #{stamp}"
- p "@transforms[frame_id] = #{@transforms[frame_id]}"
if not @transforms[frame_id]
- return nil
+ return Transform.new([0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 1.0],
+ nil, frame_id, stamp)
end
if not stamp or stamp == ROS::Time.new
# latest
- @transforms[frame_id].last
+ return @transforms[frame_id].last
else
@transforms[frame_id].each do |trans|
- if stamp > trans.stamp
+ if stamp >= trans.stamp
return trans
end
end
@@ -120,11 +130,11 @@ def find_transform(frame_id, stamp=nil)
def add_transform(trans)
if @transforms[trans.frame_id]
@transforms[trans.frame_id].push(trans)
- else
- @transforms[trans.frame_id] = [trans]
if @transforms[trans.frame_id].length > @max_buffer_length
@transforms[trans.frame_id].shift
end
+ else
+ @transforms[trans.frame_id] = [trans]
end
# it is better to set parent again?
end
2  rosruby_tf/test/test_broadcaster.rb
View
@@ -17,7 +17,7 @@ def test_broadcast
sleep 1
now = ROS::Time::now
- tf_broadcaster.send_transform([1.0, 2.0, 3.0], [0.0,0.0,0.0,1.0], now, '/child', '/parent')
+ tf_broadcaster.send_transform([1.0, 2.0, 3.0], [0.0,0.0,0.0,1.0], now, '/parent', '/child')
sleep 1
node.spin_once
44 rosruby_tf/test/test_listener.rb
View
@@ -0,0 +1,44 @@
+#!/usr/bin/env ruby
+
+require 'ros'
+ROS::load_manifest("rosruby_tf")
+
+require 'test/unit'
+require 'tf/listener'
+require 'tf/broadcaster'
+
+class TestTfListener < Test::Unit::TestCase
+ def test_init
+ node = ROS::Node.new('/tf_listener')
+ tf_listener = Tf::TransformListener.new(node)
+ sleep 1
+ tf_broadcaster = Tf::TransformBroadcaster.new(node)
+ sleep 1
+
+ now = ROS::Time.now
+ tf_broadcaster.send_transform([0.5, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 1.0],
+ now,
+ '/base', '/shoulder')
+ tf_broadcaster.send_transform([0.0, -0.2, 0.0],
+ [0.0, 0.0, 0.0, 1.0],
+ now,
+ '/shoulder', '/hand')
+ tf_broadcaster.send_transform([0.0, 0.1, 0.1],
+ [0.0, 0.0, 0.0, 1.0],
+ now,
+ '/hand', '/head')
+ stamp = ROS::Time.new
+ sleep 1
+
+ tf = tf_listener.lookup_transform('/shoulder', '/hand', stamp)
+ assert(tf)
+
+ assert(!tf_listener.lookup_transform('/shoulder', '/hoge', stamp))
+ assert(!tf_listener.lookup_transform('/age', '/hoge', stamp))
+ assert(!tf_listener.lookup_transform('/age', '/shoulder', stamp))
+
+ tf_hand = tf_listener.lookup_transform('/base', '/hand', stamp)
+ assert(tf_hand)
+ end
+end
34 rosruby_tf/test/test_transformer.rb
View
@@ -13,12 +13,12 @@ def setup
# | |
# | -- frame3
# -- framea ---- frameb
- @root = Tf::Transform.new('/root', [0, 0, 0], [0, 0, 0, 1], nil)
- @frame1 = Tf::Transform.new('/frame1', [1, 0, 0], [0, 0, 0, 1], @root)
- @frame2 = Tf::Transform.new('/frame2', [1, 0, 0], [0, 0, 0, 1], @frame1)
- @frame3 = Tf::Transform.new('/frame3', [0, -1, 0], [0, 0, 0, 1], @frame1)
- @framea = Tf::Transform.new('/framea', [-1, 0, 0], [0, 0, 0, 1], @root)
- @frameb = Tf::Transform.new('/frameb', [1, 1, 0], [0, 0, 0, 1], @framea)
+ @root = Tf::Transform.new([0, 0, 0], [0, 0, 0, 1], nil, '/root')
+ @frame1 = Tf::Transform.new([1, 0, 0], [0, 0, 0, 1], @root, '/frame1')
+ @frame2 = Tf::Transform.new([1, 0, 0], [0, 0, 0, 1], @frame1, '/frame2')
+ @frame3 = Tf::Transform.new([0, -1, 0], [0, 0, 0, 1], @frame1, '/frame3')
+ @framea = Tf::Transform.new([-1, 0, 0], [0, 0, 0, 1], @root, '/framea')
+ @frameb = Tf::Transform.new([1, 1, 0], [0, 0, 0, 1], @framea, '/frameb')
end
def test_root
@@ -40,3 +40,25 @@ def test_transform_chain
puts @root.get_transform_to(@frame3)
end
end
+
+class TestTransformBuffer < Test::Unit::TestCase
+ def test_initialize
+ buf = Tf::TransformBuffer.new(10)
+ assert_equal(10, buf.max_buffer_length)
+ buf.max_buffer_length = 100
+ assert_equal(100, buf.max_buffer_length)
+ end
+
+ def test_add_find_transform
+ buf = Tf::TransformBuffer.new(100)
+ trans = Tf::Transform.new([0.1, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0], nil, '/base', ROS::Time.new)
+ now = ROS::Time.now
+ trans_now = Tf::Transform.new([0.1, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0], nil, '/base', now)
+# assert(!buf.find_transform('/base'))
+ buf.add_transform(trans)
+ assert_equal(trans, buf.find_transform('/base'))
+ buf.add_transform(trans_now)
+ assert_equal(trans_now, buf.find_transform('/base'))
+ assert_equal(trans, buf.find_transform('/base', now))
+ end
+end
6 rosruby_tutorials/bin/tf_broadcaster.rb
View
@@ -10,14 +10,14 @@
r = ROS::Rate.new(1.0)
while node.ok?
now = ROS::Time.now
- tf_broadcaster.send_transform([0.1, 0.0, 0.0],
+ tf_broadcaster.send_transform([0.5, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0],
now,
- '/shoulder', '/base')
+ '/base', '/shoulder')
tf_broadcaster.send_transform([0.0, -0.2, 0.0],
[0.0, 0.0, 0.0, 1.0],
now,
- '/hand', '/shoulder')
+ '/shoulder', '/hand')
tf_broadcaster.send_transform([0.0, 0.1, 0.1],
[0.0, 0.0, 0.0, 1.0],
now,
9 rosruby_tutorials/bin/tf_listener.rb
View
@@ -11,9 +11,14 @@
while node.ok?
stamp = ROS::Time.new
+ tf = tf_listener.lookup_transform('/shoulder', '/hand', stamp)
+ if tf
+ node.loginfo("TF Shoulder->Hand=#{tf}")
+ end
+
tf = tf_listener.lookup_transform('/base', '/hand', stamp)
- if tf
- node.loginfo("#{tf}")
+ if tf
+ node.loginfo("TF Base->Hand=#{tf}")
end
r.sleep
Please sign in to comment.
Something went wrong with that request. Please try again.