pub_3d_target.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     pub_3d_target.py - Version 1.0 2011-08-17
00005     
00006     Publish a target point relative to a given reference frame.
00007     
00008     Created for the Pi Robot Project: http://www.pirobot.org
00009     Copyright (c) 2010 Patrick Goebel.  All rights reserved.
00010 
00011     This program is free software; you can redistribute it and/or modify
00012     it under the terms of the GNU General Public License as published by
00013     the Free Software Foundation; either version 2 of the License, or
00014     (at your option) any later version.
00015     
00016     This program is distributed in the hope that it will be useful,
00017     but WITHOUT ANY WARRANTY; without even the implied warranty of
00018     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019     GNU General Public License for more details at:
00020     
00021     http://www.gnu.org/licenses/gpl.html
00022 """
00023 
00024 import roslib; roslib.load_manifest('pi_head_tracking_3d_part2')
00025 import rospy
00026 import tf
00027 import sys
00028 from visualization_msgs.msg import Marker
00029 from geometry_msgs.msg import PointStamped, Point
00030 import math
00031 import random
00032 
00033 class Pub3DTarget():
00034     def __init__(self):
00035         rospy.init_node('pub_3d_target')
00036                 
00037         rate = rospy.get_param('~rate', 10)
00038         r = rospy.Rate(rate)
00039         
00040         # Remap this frame in the launch file or command line if necessary
00041         target_frame = 'plate_3_link'
00042         target_topic = 'target_point'
00043         
00044         # Parameters for publishing the RViz target marker
00045         marker_scale = rospy.get_param('~marker_scale', 0.05)
00046         marker_lifetime = rospy.get_param('~marker_lifetime', 1) # 0 is forever
00047         marker_ns = rospy.get_param('~marker_ns', 'target_point')
00048         marker_id = rospy.get_param('~marker_id', 0)
00049         marker_color = rospy.get_param('~marker_color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0})
00050         
00051         # Define a marker publisher
00052         marker_pub = rospy.Publisher('target_point_marker', Marker)
00053         
00054         # Initialize the marker
00055         marker = Marker()
00056         marker.ns = marker_ns
00057         marker.id = marker_id
00058         marker.type = Marker.SPHERE
00059         marker.action = Marker.ADD
00060         marker.lifetime = rospy.Duration(marker_lifetime)
00061         marker.scale.x = marker_scale
00062         marker.scale.y = marker_scale
00063         marker.scale.z = marker_scale
00064         marker.color.r = marker_color['r']
00065         marker.color.g = marker_color['g']
00066         marker.color.b = marker_color['b']
00067         marker.color.a = marker_color['a']
00068         
00069         # Define the target as a PointStamped message
00070         target = PointStamped()
00071         target.header.frame_id = target_frame
00072         
00073         # Define the target publisher
00074         target_pub = rospy.Publisher(target_topic, PointStamped)
00075 
00076         # Initialize tf listener       
00077         tf_listener = tf.TransformListener()
00078         
00079         # Give the tf buffer a chance to fill up
00080         rospy.sleep(5.0)
00081         
00082         rospy.loginfo("Publishing target point on frame " + str(target_frame))
00083         
00084         theta = 0.0
00085         rand_num = random.Random()
00086         
00087         while not rospy.is_shutdown():
00088             target.point.x = 0.5 + 0.25 * math.sin(theta)
00089             target.point.y = 0.5 * math.sin(theta)
00090             target.point.z = -0.1 + 0.5 * abs(math.sin(theta))
00091             theta += 0.1
00092             target.header.stamp = rospy.Time.now()
00093             target_pub.publish(target)
00094             
00095             marker.header.stamp = target.header.stamp
00096             marker.header.frame_id = target.header.frame_id
00097             marker.pose.position = target.point
00098             marker.id += 1
00099             marker.id %= 100
00100             marker_pub.publish(marker)                       
00101 
00102             r.sleep()
00103                    
00104 if __name__ == '__main__':
00105     try:
00106         target = Pub3DTarget()
00107         rospy.spin()
00108     except rospy.ROSInterruptException:
00109         rospy.loginfo("Target publisher is shut down.")


pi_head_tracking_3d_part2
Author(s): Patrick Goebel
autogenerated on Mon Oct 6 2014 03:26:53