Created
May 8, 2015 14:54
-
-
Save awesomebytes/f357f75d7162f936b5db to your computer and use it in GitHub Desktop.
look at own hand tiago
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#! /usr/bin/env python | |
# -*- coding: utf-8 -*- | |
""" | |
Created on 5/8/15 | |
@author: sampfeiffer | |
look_hand.py contains... | |
""" | |
__author__ = 'sampfeiffer' | |
# System imports | |
# Local imports | |
# ROS imports | |
import rospy | |
import tf.transformations | |
# ROS messages imports | |
from control_msgs.msg import PointHeadActionGoal | |
from geometry_msgs.msg import PointStamped | |
# Useful colors for prints | |
HEADER = '\033[95m' | |
OKBLUE = '\033[94m' | |
OKGREEN = '\033[92m' | |
WARNING = '\033[93m' | |
FAIL = '\033[91m' | |
ENDC = '\033[0m' | |
LOOK_TO_POINT_AS_GOAL_TOPIC = '/head_controller/point_head_action/goal' | |
class LookAtMyHand(): | |
"""This class does stuff""" | |
def __init__(self): | |
# Topics | |
rospy.loginfo("Setting publisher to " + LOOK_TO_POINT_AS_GOAL_TOPIC) | |
self.pub_head_topic = rospy.Publisher(LOOK_TO_POINT_AS_GOAL_TOPIC, PointHeadActionGoal, queue_size=1) | |
self.tf_l = tf.TransformListener() | |
def run(self): | |
r = rospy.Rate(5) | |
phag = PointHeadActionGoal() | |
phag.header.frame_id = "/base_link" | |
phag.goal.max_velocity = 1.0 | |
phag.goal.min_duration = 1.0 | |
phag.goal.target.header.frame_id = "/base_link" | |
phag.goal.pointing_axis.x = 1.0 | |
phag.goal.pointing_frame = "head_2_link" | |
while not rospy.is_shutdown(): | |
ps = PointStamped() | |
ps.header.stamp = self.tf_l.getLatestCommonTime("base_link", "arm_7_link") | |
ps.header.frame_id = "/arm_7_link" | |
transform_ok = False | |
while not transform_ok and not rospy.is_shutdown(): | |
try: | |
arm7link_ps = self.tf_l.transformPoint("/base_link", ps) | |
transform_ok = True | |
except tf.ExtrapolationException as e: | |
rospy.logwarn("Exception on transforming point... trying again \n(" + str(e) + ")") | |
rospy.sleep(0.01) | |
ps.header.stamp = self.tf_l.getLatestCommonTime("base_link", "arm_7_link") | |
phag.goal.target.point = arm7link_ps.point | |
rospy.loginfo("Sending: " + str(phag)) | |
self.pub_head_topic.publish(phag) | |
r.sleep() | |
if __name__ == '__main__': | |
rospy.init_node('look_at_my_hand') | |
node = LookAtMyHand() | |
#rospy.spin() | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment