Beispiel #1
0
    def look_at(self, x, y, z, frame):
        #The goal message will be sending
        goal = PointHeadGoal()
        point = PointStamped()

        #The target point, expressed in the requested frame
        point.header.frame_id = frame
        point.point.x = x
        point.point.y = y
        point.point.z = z

        goal.target = point

        #We are pointing the high-def camera frame to default X-axis
        goal.pointing_frame = "base_link"
        goal.pointing_axis.x = 1
        goal.pointing_axis.y = 0
        goal.pointing_axis.z = 0

        #Take at least 0.5 seconds to get there
        goal.min_duration = rospy.Duration(0.5)
        #Go no faster than 1 rad/s
        goal.max_velocity = 1.0

        #send the goal
        self.client.send_goal_and_wait(goal, rospy.Duration(3))
Beispiel #2
0
 def move_head_hor (self, hor_val):
     head_goal = PointHeadGoal()
     head_goal.target.header.frame_id = 'torso_lift_link'
     head_goal.max_velocity = .3
     self.hor_pos = -(hor_val/20.0 - 2.5)
     head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
     
     self.head_client.send_goal(head_goal)
     if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
         rospy.logwarn('Head action unsuccessful.')
Beispiel #3
0
 def look_at(self, x, y, z, frame, max_v, duration=0.5):
     goal = PointHeadGoal()
     goal.target.header.stamp = rospy.Time.now()
     goal.target.header.frame_id = frame
     goal.target.point.x = x
     goal.target.point.y = y
     goal.target.point.z = z
     goal.min_duration = rospy.Duration(duration)
     goal.max_velocity = max_v
     self.client.send_goal(goal)
Beispiel #4
0
    def SetPointHead(self, point):
        goal = PointHeadGoal()
        corrected_point = self.tf.transformPoint("head_pan_link", point)
        rospy.loginfo(corrected_point)
        goal.target = corrected_point
        goal.min_duration = rospy.Duration(.5)
        goal.max_velocity = 1

        self.client.send_goal(goal)
        self.last_trigger_time = rospy.Time.now()
def lookAt(self, point):
    # Create tf transformer for z point transformation to keep TIAGo's head on the same level
    tfBuffer = tf2_ros.Buffer()
    tf = tf2_ros.TransformListener(tfBuffer)

    # Wait for the server to come up
    rospy.loginfo('Waiting for the point head server to come up')
    self.point_head_client.wait_for_server(rospy.Duration(10.0))

    # Create the goal
    print('Looking at: ', point)
    ph_goal = PointHeadGoal()
    ph_goal.target.header.frame_id = 'map'
    ph_goal.max_velocity = 1
    ph_goal.min_duration = rospy.Duration(0.5)
    ph_goal.target.point.x = point[0]
    ph_goal.target.point.y = point[1]

    ph_goal.pointing_frame = 'head_2_link'
    ph_goal.pointing_axis.x = 1
    ph_goal.pointing_axis.y = 0
    ph_goal.pointing_axis.z = 0

    ps = PointStamped()
    ps.header.stamp = rospy.Time(0)
    ps.header.frame_id = 'head_2_link'
    transform_ok = False
    while not transform_ok and not rospy.is_shutdown():
        try:
            transform = tfBuffer.lookup_transform('base_link', 'head_2_link',
                                                  rospy.Time(0))
            get_z_ps = do_transform_point(ps, transform)
            transform_ok = True
        # This usually happens only on startup
        except tf2_ros.ExtrapolationException as e:
            rospy.sleep(1.0 / 4)
            ps.header.stamp = rospy.Time(0)
            rospy.logwarn(
                "Exception on transforming point... trying again \n(" +
                str(e) + ") at time " + str(ps.header.stamp))
        except tf2_ros.LookupException:
            pass
        except tf2_ros.ConnectivityException:
            pass

    ph_goal.target.point.z = get_z_ps.point.z
    print(get_z_ps.point.z)

    # Send the goal
    rospy.loginfo("Sending the goal...")
    self.point_head_client.send_goal(ph_goal)
    rospy.loginfo("Goal sent!!")

    rospy.sleep(3)
Beispiel #6
0
    def look_at(self, x, y, z):
        phg = PointHeadGoal()
        phg.target.header.frame_id = '/stereo_optical_frame'
        phg.target.point.x = x
        phg.target.point.y = y
        phg.target.point.z = z

        phg.pointing_axis.z = 1.0
        phg.pointing_frame = phg.target.header.frame_id
        phg.min_duration = rospy.Duration(1.0)
        phg.max_velocity = 1.0

        self.point_head_ac.send_goal_and_wait(phg)
    def lookAt(self, point):
        rospy.loginfo("sending look command %s" % point.point)
        goal = PointHeadGoal()
        goal.target = point
        goal.pointing_axis.x = 1.0
        goal.pointing_frame = "kinect2_link"
        goal.min_duration.secs = 2.0
        goal.max_velocity = 1.0
        
        action_goal = PointHeadActionGoal()
        action_goal.goal = goal

        self.pub.publish(action_goal)
    def look_at(self, x, y, z):
        phg = PointHeadGoal()
        phg.target.header.frame_id = '/stereo_optical_frame'
        phg.target.point.x = x
        phg.target.point.y = y
        phg.target.point.z = z

        phg.pointing_axis.z = 1.0
        phg.pointing_frame = phg.target.header.frame_id
        phg.min_duration = rospy.Duration(1.0)
        phg.max_velocity = 1.0

        self.point_head_ac.send_goal_and_wait(phg)
Beispiel #9
0
  def lookat_goal(self, angle):
    head_goal = PointHeadGoal()
    head_goal.target.header.frame_id = '/torso_lift_link'
    head_goal.max_velocity = 0.8

    angle_in_radians = math.radians(angle)
    x = math.cos(angle_in_radians) * 5
    y = math.sin(angle_in_radians) * 5
    z = -0.5
    
    head_goal.target.point = Point(x, y, z)

    return head_goal
    def point_head(self, x, y, z):
        """
        Point the head to the specified point
        """
        head_goal = PointHeadGoal()
        head_goal.target.header.frame_id = '/torso_lift_link'
        head_goal.max_velocity = .3
        # The transform seems to aim high. Move it down a little...
        head_goal.target.point = Point(x, y, z - .4)

        rospy.logdebug('Moving head to\n' + str(head_goal.target.point))
        
        self.head_client.send_goal(head_goal)
Beispiel #11
0
    def SetPointHead(self, point):
        goal = PointHeadGoal()
        now = rospy.Time.now()  # point.header.stamp
        rospy.loginfo(
            self.tf.waitForTransform('head_pan_link', 'base_link', now,
                                     rospy.Duration(4.0)))
        corrected_point = self.tf.transformPoint('head_pan_link', point)
        corrected_point.point.z = 0.0
        rospy.loginfo(corrected_point)
        goal.target = corrected_point
        goal.min_duration = rospy.Duration(.5)
        goal.max_velocity = 1

        self.client.send_goal_and_wait(goal)
Beispiel #12
0
    def look_at(self, target_point, frame='base_footprint'):
        # make tiago look at a point
        point_head_goal = PointHeadGoal()
        point_head_goal.target.header.frame_id = frame
        point_head_goal.max_velocity = 1
        point_head_goal.min_duration = rospy.Duration(2.0)
        point_head_goal.target.header.stamp = rospy.Time(0)
        point_head_goal.target.point = target_point
        point_head_goal.pointing_frame = 'head_2_link'
        point_head_goal.pointing_axis = Vector3(1, 0, 0)

        self.point_head.send_goal_and_wait(point_head_goal)
        rospy.loginfo(
            'Sent point_head goal and waiting for robot to carry it out... ')
Beispiel #13
0
    def look_at_r_gripper(self):
        name_space = '/head_traj_controller/point_head_action'

        head_client = SimpleActionClient(name_space, PointHeadAction)
        head_client.wait_for_server()

        head_goal = PointHeadGoal()
        head_goal.target.header.frame_id = "r_gripper_tool_frame"
        head_goal.min_duration = rospy.Duration(0.3)
        head_goal.target.point = Point(0, 0, 0)
        head_goal.max_velocity = 1.0
        head_client.send_goal(head_goal)
        head_client.wait_for_result(rospy.Duration(5.0))

        if (head_client.get_state() != GoalStatus.SUCCEEDED):
            rospy.logwarn('Head action unsuccessful.')
Beispiel #14
0
    def look_at_r_gripper(self):
        name_space = '/head_traj_controller/point_head_action'

        head_client = SimpleActionClient(name_space, PointHeadAction)
        head_client.wait_for_server()

        head_goal = PointHeadGoal()
        head_goal.target.header.frame_id = "r_gripper_tool_frame"
        head_goal.min_duration = rospy.Duration(0.3)
        head_goal.target.point = Point(0, 0, 0)
        head_goal.max_velocity = 1.0
        head_client.send_goal(head_goal)
        head_client.wait_for_result(rospy.Duration(5.0))

        if (head_client.get_state() != GoalStatus.SUCCEEDED):
            rospy.logwarn('Head action unsuccessful.')
Beispiel #15
0
        def move_head():
            name_space = '/head_traj_controller/point_head_action'

            head_client = SimpleActionClient(name_space, PointHeadAction)
            head_client.wait_for_server()

            head_goal = PointHeadGoal()
            head_goal.target.header.frame_id = self.get_frame()
            head_goal.min_duration = rospy.Duration(0.3)
            head_goal.target.point = self.get_target()
            head_goal.max_velocity = 10.0
            head_client.send_goal(head_goal)
            head_client.wait_for_result(rospy.Duration(2.0))

            if (head_client.get_state() != GoalStatus.SUCCEEDED):
                rospy.logwarn('Head action unsuccessful.')
            
            self.gui.show_text_in_rviz("Head!")
Beispiel #16
0
        def move_head():
            name_space = '/head_traj_controller/point_head_action'

            head_client = SimpleActionClient(name_space, PointHeadAction)
            head_client.wait_for_server()

            head_goal = PointHeadGoal()
            head_goal.target.header.frame_id = self.get_frame()
            head_goal.min_duration = rospy.Duration(0.3)
            head_goal.target.point = self.get_target()
            head_goal.max_velocity = 10.0
            head_client.send_goal(head_goal)
            head_client.wait_for_result(rospy.Duration(2.0))

            if (head_client.get_state() != GoalStatus.SUCCEEDED):
                rospy.logwarn('Head action unsuccessful.')
            
            self.gui.show_text_in_rviz("Head!")
Beispiel #17
0
	def _look_at(self, parent_frame, x, y, z):
		goal = PointHeadGoal()
		#the point to be looking at is expressed in the "base_link" frame
		point = PointStamped()
		point.header.frame_id = parent_frame
		point.header.stamp = rospy.Time.now()
		point.point.x = x
		point.point.y = y 
		point.point.z = z
		goal.target = point

		#we want the X axis of the camera frame to be pointing at the target
		goal.pointing_frame = "high_def_frame"
		goal.pointing_axis.x = 1
		goal.pointing_axis.y = 0
		goal.pointing_axis.z = 0
		# goal.min_duration = rospy.Duration(2.0)
		goal.max_velocity = 0.85
		self.client.send_goal(goal)
Beispiel #18
0
def main():
    rospy.init_node('ball_tracking', anonymous=True)
    head_client = actionlib.SimpleActionClient(
        "/head_controller/absolute_point_head_action", PointHeadAction)
    head_client.wait_for_server()
    goal = PointHeadGoal()
    goal.target.header.stamp = rospy.Time.now()
    goal.target.header.frame_id = "/camera_color_optical_frame"
    goal.pointing_axis.x = 0
    goal.pointing_axis.y = 0
    goal.pointing_axis.z = 1

    goal.target.point.x = 0.7
    goal.target.point.y = 0
    goal.target.point.z = 0.4
    goal.max_velocity = 0.1
    goal.min_duration = rospy.Duration(2.0)
    head_client.send_goal(goal)
    head_client.wait_for_result()
Beispiel #19
0
    def _look_at(self, parent_frame, x, y, z):
        goal = PointHeadGoal()
        #the point to be looking at is expressed in the "base_link" frame
        point = PointStamped()
        point.header.frame_id = parent_frame
        point.header.stamp = rospy.Time.now()
        point.point.x = x
        point.point.y = y
        point.point.z = z
        goal.target = point

        #we want the X axis of the camera frame to be pointing at the target
        goal.pointing_frame = "high_def_frame"
        goal.pointing_axis.x = 1
        goal.pointing_axis.y = 0
        goal.pointing_axis.z = 0
        # goal.min_duration = rospy.Duration(2.0)
        goal.max_velocity = 0.85
        self.client.send_goal(goal)
Beispiel #20
0
def tiago_point_head(x, y, wait=False):
    point_head_client = actionlib.SimpleActionClient(
        '/head_controller/point_head_action', PointHeadAction)
    point_head_client.wait_for_server()
    chair_point = Point(x, y, 1)

    # create head goal
    ph_goal = PointHeadGoal()
    ph_goal.target.header.frame_id = 'map'
    ph_goal.max_velocity = 1
    ph_goal.min_duration = rospy.Duration(0.5)
    ph_goal.target.header.stamp = rospy.Time(0)
    ph_goal.target.point = chair_point
    ph_goal.pointing_frame = 'head_2_link'
    ph_goal.pointing_axis = Vector3(1, 0, 0)

    point_head_client.send_goal(ph_goal)
    if wait:
        point_head_client.wait_for_result()
Beispiel #21
0
    def tilt_head(self, down=True):
        name_space = '/head_traj_controller/point_head_action'

        head_client = SimpleActionClient(name_space, PointHeadAction)
        head_client.wait_for_server()

        head_goal = PointHeadGoal()
        head_goal.target.header.frame_id = self.get_frame()
        head_goal.min_duration = rospy.Duration(0.3)
        if down:
            head_goal.target.point = Point(1, 0, Head.speed * -0.1)
        else:
            head_goal.target.point = Point(1, 0, Head.speed * 0.1)
        head_goal.max_velocity = 10.0
        head_client.send_goal(head_goal)
        head_client.wait_for_result(rospy.Duration(2.0))

        if (head_client.get_state() != GoalStatus.SUCCEEDED):
            rospy.logwarn('Head action unsuccessful.')
Beispiel #22
0
    def lookatpoint(self, x, y, z, velocity=10.8):
        head_client = actionlib.SimpleActionClient(
            "/head_controller/absolute_point_head_action", PointHeadAction)
        head_client.wait_for_server()
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = "/camera_color_optical_frame"
        goal.pointing_axis.x = 0
        goal.pointing_axis.y = 0
        goal.pointing_axis.z = 1

        goal.target.point.x = x
        goal.target.point.y = y
        goal.target.point.z = z
        goal.max_velocity = velocity
        goal.min_duration = rospy.Duration(1.0)

        ## motion start
        if self.isMotion == False:
            head_client.send_goal(goal)
            self.isMotion = True
        rospy.sleep(0.5)
Beispiel #23
0
    def SetPointHead(self, point):
        goal = PointHeadGoal()
        corrected_point = self.tf.transformPoint("head_pan_link", point)
        rospy.loginfo(corrected_point)
        goal.target = corrected_point
        goal.min_duration = rospy.Duration(.5)
        goal.max_velocity = 1

        # send the goal to the server
        self.ignore_inputs = True
        self.client.send_goal(goal)
        # block and wait for the turn to be finished
        # doing this because there are a lot of noises when turning the head
        # ignore the input during that period by blocking here
        result = self.client.wait_for_result(rospy.Duration(0.0))
        rospy.loginfo("Result:")
        rospy.loginfo(result)
        rospy.sleep(2)
        self.ignore_inputs = False

        # re-initialized history array, buffer array and counter
        self.angle = np.array([])
        self.buf = np.zeros(BUFFER_LENGTH)
        self.counter = 0
Beispiel #24
0
    marker.action = Marker.ADD
        
    marker.pose.position.x = 1
    marker.pose.position.y = 0
    marker.pose.position.z = 0

    marker.scale.x = 0.1
    marker.scale.y = 0.1
    marker.scale.z = 0.1
    marker.color.a = 1.0
    marker.color.r = 1.0
    marker.color.g = 0.0
    marker.color.b = 0.0
    
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        gaze_point = PointHeadGoal()
        gaze_point.target.header.frame_id = 'glass'
        gaze_point.pointing_frame = 'high_def_optical_frame'
        gaze_point.min_duration = rospy.Duration(0.01)
        gaze_point.max_velocity = 1.0
        gaze_point.target.header.stamp = rospy.Time.now()
        gaze_point.target.point.x = 1
        gaze_point.target.point.y = 0
        gaze_point.target.point.z = 0
        head_client.send_goal(gaze_point)

        pub.publish(marker)

        rate.sleep()