Launch file for NUSSEDS Autonomy x Rovers Software bootcamp

robot_tf_tree.launch

<launch>
<node pkg="tf" type="static_transform_publisher" name="baselink2camera" args="0 0 1.2 0 0 0 1 base_link camera_link 100" />
<node pkg="tf" type="static_transform_publisher" name="baselink2gripper" args="0.5 0 0.6 0 0 0 1 base_link gripper 100" />
<node pkg="rostopic" type="rostopic" name="apple_detector" args="pub /camera/detector/apple geometry_msgs/PointStamped &quot;{header:
  {seq: 0,
  stamp: {
    secs: 0,
    nsecs: 0},
  frame_id: &apos;camera_link&apos;},
point:{
  x: 1.0,
  y: 0.0,
  z: -0.8}}&quot; -r10
" output="screen"/>
</launch>

tf_demo.py

import rospy
import tf
from geometry_msgs.msg import PointStamped

def on_recieve_point(pt):
    print pt

rospy.init_node("transform_to_gripper")

sub = rospy.Subscriber("/camera/detector/apple", PointStamped, on_recieve_point)
rospy.spin()
~            

tf_demo_finished.py

import rospy
import tf
from geometry_msgs.msg import PointStamped



rospy.init_node("transform_to_gripper")
listener = tf.TransformListener()
def on_recieve_point(point):
    try:
        transformed_point = listener.transformPoint("gripper", point)
        print (transformed_point)
    except tf.Exception as e:
        print (e)
sub = rospy.Subscriber("/camera/detector/apple", PointStamped, on_recieve_point)
rospy.spin()