def point_head_to(self, position, frame, pointing_frame=None, pointing_axis=None):
        """Point the head to the (x,y,z) tuple specified by position in the
        passed frame. Default pointing frame is head_tilt_link with axis
        [1,0,0].
        """
        goal = PointHeadGoal()

        if pointing_frame is None:
            pointing_frame = "head_tilt_link"
        if pointing_axis is None:
            pointing_axis = [1, 0, 0]

        goal.target.header.frame_id = frame
        goal.target.point.x = position[0]
        goal.target.point.y = position[1]
        goal.target.point.z = position[2]

        goal.pointing_frame = pointing_frame
        goal.pointing_axis.x = pointing_axis[0]
        goal.pointing_axis.y = pointing_axis[1]
        goal.pointing_axis.z = pointing_axis[2]

        goal.min_duration = rospy.Duration(self.time_to_reach)

        self.head_pointer_client.send_goal_and_wait(goal, rospy.Duration(self.time_to_reach))
Esempio n. 2
0
    def loop(self):
        while not rospy.is_shutdown():
            if self.marker is not None:
                self.debug.publish(self.marker)

            # Reset head if no face found in 1 second
            if rospy.Time.now() - self.lastFaceTimestamp > rospy.Duration(1):
                self.lastHeadTarget = None

                # Reset head
                pos = PointStamped()
                pos.header.stamp = rospy.Time.now()
                pos.header.frame_id = "base_link"
                if self.controllerPosition is None:
                    pos.point.x = 1
                    pos.point.y = 0
                else:
                    pos.point.x = self.controllerPosition.x
                    pos.point.y = self.controllerPosition.y
                pos.point.z = 1.5
                goal = PointHeadGoal()
                goal.min_duration = rospy.Duration(0.5)
                goal.target = pos
                self.head_client.send_goal(goal)

            rospy.sleep(0.1)
Esempio n. 3
0
    def loop(self):
        while not rospy.is_shutdown():
            if self.has_goal and self.enable_tilt:
                goal = PointHeadGoal()
                goal.target.header.stamp = rospy.Time.now()
                goal.target.header.frame_id = 'base_link'
                with self.mutex:
                    goal.target.point.x = self.x
                    goal.target.point.y = self.y
                    self.x = 1
                    self.y = 0
                goal.target.point.z = 0.75
                goal.min_duration = rospy.Duration(1.0)

                self.client.send_goal(goal)
                self.client.wait_for_result()

                with self.mutex:
                    goal.target.point.x = self.x
                    goal.target.point.y = self.y
                    self.x = 1
                    self.y = 0
                goal.target.point.z = 0.0

                self.client.send_goal(goal)
                self.client.wait_for_result()
            else:
                rospy.sleep(1.0)
Esempio n. 4
0
    def loop(self):
        while not rospy.is_shutdown():
            if abs((rospy.Time.now() - self.last_vel_time).to_sec()) < 1.0:
                goal = PointHeadGoal()
                goal.target.header.stamp = rospy.Time.now()
                goal.target.header.frame_id = "base_link"
                with self.mutex:
                    goal.target.point.x = self.x
                    goal.target.point.y = self.y
                    self.x = 1
                    self.y = 0
                goal.target.point.z = 0.0
                goal.min_duration = rospy.Duration(1.0)

                self.client.send_goal(goal)
                self.client.wait_for_result()

                with self.mutex:
                    goal.target.point.x = self.x
                    goal.target.point.y = self.y
                    self.x = 1
                    self.y = 0
                goal.target.point.z = 0.75

                self.client.send_goal(goal)
                self.client.wait_for_result()
            else:
                rospy.sleep(1.0)
Esempio n. 5
0
    def loop(self):
        while not rospy.is_shutdown():
            if self.has_goal:
                goal = PointHeadGoal()
                goal.target.header.stamp = rospy.Time.now()
                goal.target.header.frame_id = "base_link"
                with self.mutex:
                    goal.target.point.x = self.x
                    goal.target.point.y = self.y
                    self.x = 1
                    self.y = 0
                goal.target.point.z = 0.0
                goal.min_duration = rospy.Duration(1.0)

                self.client.send_goal(goal)
                self.client.wait_for_result()

                with self.mutex:
                    goal.target.point.x = self.x
                    goal.target.point.y = self.y
                    self.x = 1
                    self.y = 0
                goal.target.point.z = 0.75

                self.client.send_goal(goal)
                self.client.wait_for_result()
            else:
                rospy.sleep(1.0)
 def head_action(self, x, y, z, wait=False):
     name_space = '/head_traj_controller/point_head_action'
     head_client = SimpleActionClient(name_space, PointHeadAction)
     head_goal = PointHeadGoal()
     head_goal.target.header.frame_id = 'base_link'
     head_goal.min_duration = rospy.Duration(1.0)
     head_goal.target.point = Point(x, y, z)
     head_client.send_goal(head_goal)
     if wait:
         # wait for the head movement to finish before we try to detect and pickup an object
         finished_within_time = head_client.wait_for_result(
             rospy.Duration(5))
         # Check for success or failure
         if not finished_within_time:
             head_client.cancel_goal()
             rospy.loginfo("Timed out achieving head movement goal")
         else:
             state = head_client.get_state()
             if state == GoalStatus.SUCCEEDED:
                 rospy.loginfo("Head movement goal succeeded!")
                 rospy.loginfo("State:" + str(state))
             else:
                 rospy.loginfo(
                     "Head movement goal failed with error code: " +
                     str(self.goal_states[state]))
Esempio n. 7
0
 def execute(self, userdata):
     rospy.loginfo('Executing Move head')
     
     frame_id = self.s_frame_id if self.s_frame_id else userdata.target_frame_id
     point_to_look = self.s_point_to_look if self.s_point_to_look else userdata.target_point 
     
     look_goal = PointHeadGoal()
     look_goal.target.header.frame_id = frame_id
     look_goal.target.point.x = point_to_look[0]
     look_goal.target.point.y = point_to_look[1]
     look_goal.target.point.z = point_to_look[2]
     
     look_goal.pointing_frame = POINTING_FRAME 
     if POINTING_FRAME == 'head_mount_xtion_rgb_optical_frame':
         look_goal.pointing_axis.x = 0.0
         look_goal.pointing_axis.y = 0.0
         look_goal.pointing_axis.z = 1.0
     elif POINTING_FRAME == 'stereo_link':
         look_goal.pointing_axis.x = 1.0
         look_goal.pointing_axis.y = 0.0
         look_goal.pointing_axis.z = 0.0
     
     look_goal.min_duration.secs = 1.0
     
     rospy.loginfo('Sending this goal: ' + str(look_goal))
     
     userdata.point_goal = look_goal
            
     return 'succeeded'
Esempio n. 8
0
    def personCallback(self, person):
        # Get the distance of this person from the robot
        person_distance = math.sqrt(person.pose.position.x ** 2 + person.pose.position.y ** 2)
        person_angle = math.degrees(math.atan2(person.pose.position.y, person.pose.position.x))

        # First look at the closest person if we know that the person is being
        # detected using the face detector
        if person.detection_context.pose_source == DetectionContext.POSE_FROM_FACE:
            self.lastFaceTimestamp = rospy.Time.now()

            # Point to the person if the distance between their current position
            # and the position we were looking at earlier has changed by more
            # than 2 cm
            goal = PointHeadGoal()
            goal.min_duration = rospy.Duration(0.2)
            goal.target = PointStamped(header=person.header, point=person.pose.position)
            head_distance = 999999999
            if self.lastHeadTarget is not None:
                head_distance = math.sqrt(
                    (person.pose.position.x - self.lastHeadTarget.point.x) ** 2
                    + (person.pose.position.y - self.lastHeadTarget.point.y) ** 2
                    + (person.pose.position.z - self.lastHeadTarget.point.z) ** 2
                )
            if head_distance > 0.02:
                self.lastHeadTarget = goal.target
                self.head_client.send_goal(goal)

            # Regardless of whether they were the original controller or not,
            # this person is the new controller if they are within 1m of us
            # if person_distance <= 1.0:
            if self.controllerID != person.id:
                rospy.loginfo("Setting controller ID to {}".format(person.id))
            self.controllerID = person.id
            self.controllerPosition = person.pose.position

        # Then check to see if the person we were tracking still in view.
        if self.controllerID == person.id:
            self.controllerPosition = person.pose.position

            # Check to see if we are not in collision. If so, MOVE!!!
            if self.safeToTrack:
                MAX_SPEED = 0.7
                cmd = Twist()

                if person_distance > 1:
                    targetSpeed = 0.15 * person_distance + 0.1
                    cmd.linear.x = min(targetSpeed, MAX_SPEED)
                elif person_distance < 0.7:
                    cmd.linear.x = -0.3

                if abs(person_angle) > 5:
                    cmd.angular.z = person_angle / 50
                self.cmdvel.publish(cmd)

        # Otherwise, we should stop trying to follow the person
        elif self.controllerID is not None:
            rospy.loginfo("Lost controller {}".format(self.controllerID))
            self.controllerID = None
            self.controllerPosition = None
 def createPointHeadActionGoal(self):
     phag = PointHeadGoal()
     phag.pointing_frame = 'stereo_link'
     phag.pointing_axis = Vector3(1.0, 0.0, 0.0)
     phag.min_duration = rospy.Duration(0.5)
     phag.target.header = Header(frame_id='oculus', stamp=rospy.Time.now())
     phag.target.point = Point(1.0, 0.0, 0.0)
     return phag
 def createPointHeadActionGoal(self):
     phag = PointHeadGoal()
     phag.pointing_frame = 'stereo_link'
     phag.pointing_axis = Vector3(1.0, 0.0, 0.0)
     phag.min_duration = rospy.Duration(0.5)
     phag.target.header = Header(frame_id='oculus', stamp=rospy.Time.now())
     phag.target.point = Point(1.0, 0.0, 0.0)
     return phag
Esempio n. 11
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.')
Esempio n. 12
0
 def execute(self, point_value):
     goal_msg = PointHeadGoal()
     point_st_msg = PointStamped()
     point_st_msg.header.frame_id = "/base_link"
     point_st_msg.header.stamp = rospy.Time.now()
     point_st_msg.point.x = point_value[0]
     point_st_msg.point.y = point_value[1]
     point_st_msg.point.z = point_value[2]
     goal_msg.target = point_st_msg
     self._client.send_goal(goal_msg)
 def look_at(self, x, y, z, frame, duration=1.0):
     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)
     self.client.send_goal(goal)
     self.client.wait_for_result()
Esempio n. 14
0
 def moveHead(self, theta, phi):
     
     head_goal = PointHeadGoal()
     head_goal.target.header.frame_id = 'base_link'
     head_goal.min_duration = rospy.Duration(1.0)
     head_goal.target.point = Point(math.cos(theta), math.sin(theta), phi)
     self.head_client.send_goal(head_goal)
     self.head_client.wait_for_result(rospy.Duration(10.0))
     if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
         rospy.logwarn('Head action unsuccessful.')
Esempio n. 15
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()
Esempio n. 16
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)
Esempio n. 17
0
 def point_head_forwards(self):
     goal = PointHeadGoal()
     goal.target.header.stamp = rospy.Time.now()
     goal.target.header.frame_id = "base_link"
     goal.target.point.x = 0.7
     goal.target.point.y = 0
     goal.target.point.z = 0.5
     goal.min_duration = rospy.Duration(1.0)
     self.head_client.send_goal(goal)
     self.head_client.wait_for_result()
 def look_at(self, x, y, z, frame, duration=1.0):
     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)
     self.client.send_goal(goal)
     self.client.wait_for_result()
Esempio n. 19
0
 def point_head_forwards(self):
     goal = PointHeadGoal()
     goal.target.header.stamp = rospy.Time.now()
     goal.target.header.frame_id = "base_link"
     goal.target.point.x = 0.7
     goal.target.point.y = 0
     goal.target.point.z = 0.5
     goal.min_duration = rospy.Duration(1.0)
     self.head_client.send_goal(goal)
     self.head_client.wait_for_result()
Esempio n. 20
0
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)
Esempio n. 21
0
    def move_to_start(self):

        self.moving_mode = True
        # Look down
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = '/base_link'
        goal.target.point.x = 1.5
        goal.target.point.y = 0.0
        goal.target.point.z = -0.2
        goal.min_duration = rospy.Duration(0.5)
        if 0: logger.info('Point head to %s...', goal)
        self.head_point_client.send_goal(goal)
        if 0: logger.info('Point head sent')

        goal = GripperCommandGoal()
        goal.command.max_effort = 60
        goal.command.position = 0.1
        self.gripper_client.send_goal(goal)

        joints = [
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
        ]
        pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
        result = self.arm_move_group.moveToJointPosition(joints,
                                                         pose,
                                                         plan_only=True)
        if result.error_code.val == MoveItErrorCodes.SUCCESS:
            if 0: logger.info('Got trajectory %s', result.planned_trajectory)
            follow_goal = FollowJointTrajectoryGoal()
            follow_goal.trajectory = result.planned_trajectory.joint_trajectory
            logger.info('sending trajectory to arm...')
            self.arm_trajectory_client.send_goal(follow_goal)
            result = self.arm_trajectory_client.wait_for_result()
            logger.info('arm followed trajectory %s', result)
        else:
            logger.warn('moveToJointPosition returned %s', result)
            return

        result = self.head_point_client.wait_for_result()
        logger.info('head followed trajectory %s', result)

        logger.info('sending empty arm goal')
        empty_goal = FollowJointTrajectoryGoal()
        self.arm_trajectory_client.send_goal(empty_goal)

        logger.info('Point head done')
        self.moving_mode = False
Esempio n. 22
0
def look_at_bin():
	head_client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
	head_client.wait_for_server()
	goal = PointHeadGoal()
	goal.target.header.stamp = rospy.Time.now()
	goal.target.header.frame_id = "base_link"
	goal.target.point.x = 0.7
	goal.target.point.y = 0
	goal.target.point.z = 0.4
	goal.min_duration = rospy.Duration(1.0)
	head_client.send_goal(goal)
	head_client.wait_for_result()
Esempio n. 23
0
    def move_to_start(self):

        self.moving_mode = True
        # Look down
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = '/base_link'
        goal.target.point.x = 1.5
        goal.target.point.y = 0.0
        goal.target.point.z = -0.2
        goal.min_duration = rospy.Duration(0.5)
        if 0: logger.info('Point head to %s...', goal);
        self.head_point_client.send_goal(goal)
        if 0: logger.info('Point head sent')

        goal = GripperCommandGoal()
        goal.command.max_effort = 60
        goal.command.position = 0.1
        self.gripper_client.send_goal(goal)

        joints = [
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
        ]
        pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
        result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
        if result.error_code.val == MoveItErrorCodes.SUCCESS:
            if 0: logger.info('Got trajectory %s', result.planned_trajectory)
            follow_goal = FollowJointTrajectoryGoal()
            follow_goal.trajectory = result.planned_trajectory.joint_trajectory
            logger.info('sending trajectory to arm...')
            self.arm_trajectory_client.send_goal(follow_goal)
            result = self.arm_trajectory_client.wait_for_result()
            logger.info('arm followed trajectory %s', result)
        else:
            logger.warn('moveToJointPosition returned %s', result)
            return

        result = self.head_point_client.wait_for_result()
        logger.info('head followed trajectory %s', result)

        logger.info('sending empty arm goal')
        empty_goal = FollowJointTrajectoryGoal()
        self.arm_trajectory_client.send_goal(empty_goal)

        logger.info('Point head done')
        self.moving_mode = False
Esempio n. 24
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)
Esempio n. 26
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 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 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)
Esempio n. 29
0
def look_at_bin():
	head_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
	head_client.wait_for_server()

	goal = PointHeadGoal()
	goal.target.header.stamp = rospy.Time.now()
	goal.target.header.frame_id = 'base_link'
	goal.target.point.x = 0.7
	goal.target.point.y = 0
	goal.target.point.z = 0.4
	goal.min_duration = rospy.Duration(1.0)

	head_client.send_goal(goal)
	head_client.wait_for_result()
Esempio n. 30
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)
Esempio n. 31
0
 def look_at(self, target_pose, duration=1.0):
     look_goal = PointHeadGoal()
     look_goal.target.header.stamp = rospy.Time.now()
     look_goal.target.header.frame_id = 'map'
     look_goal.target.point.x = target_pose.pose.orientation.x
     look_goal.target.point.y = target_pose.pose.orientation.y
     look_goal.target.point.z = target_pose.pose.orientation.z
     look_goal.min_duration = rospy.Duration(duration)
     self.client.send_goal(look_goal)
     wait = self.client.wait_for_result()
     if not wait:
         rospy.logerr('Action server for point head not available')
     else:
         rospy.loginfo('looking at the object')
def look_at_bin():
    head_client = actionlib.SimpleActionClient("head_traj_controller/point_head_action",PointHeadAction)
    print("waiting")
    head_client.wait_for_server()
    goal = PointHeadGoal()
    goal.target.header.stamp = rospy.Time.now()
    goal.target.header.frame_id = "base_link"
    goal.target.point.x = 0.7
    goal.target.point.y = -0.7
    goal.target.point.z = 0.4
    goal.min_duration = rospy.Duration(1.0)
    head_client.send_goal(goal)
    print(goal)
    head_client.wait_for_result()
    print("finished")
Esempio n. 33
0
    def look_at(self, frame_id, x, y, z):
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = frame_id
        goal.target.point.x = x
        goal.target.point.y = y
        goal.target.point.z = z

        goal.pointing_frame = "pointing_frame"
        goal.pointing_axis.x = 1
        goal.pointing_axis.y = 0
        goal.pointing_axis.z = 0

        # send goal
        self.point_head_client.send_goal(goal)
Esempio n. 34
0
    def __init__(self):
        """
        Initializes the data stream that sends the instructions to the robot and
        the point that the robot will look at.
        """
        Thread.__init__(self)
        self.daemon = True
        self.running = True

        self.rate = rospy.Rate(10)
        self.point_head_goal = SimpleActionClient(
            '/head_controller/point_head_action', PointHeadAction)

        camera_info_msg = rospy.wait_for_message('/xtion/rgb/camera_info',
                                                 CameraInfo)
        self.camera_intrinsics = array(camera_info_msg.K).reshape((3, 3))

        self.looker = PointHeadGoal()

        self.looker.target.header.frame_id = '/base_link'
        self.looker.pointing_frame = '/head_2_link'

        self.looker.pointing_axis.x = 1.0
        self.looker.pointing_axis.y = 0.0
        self.looker.pointing_axis.z = 0.0
        self.looker.max_velocity = 0.3

        look_point = PointStamped()
        look_point.header.frame_id = '/base_link'
        look_point.point.x = 40.0
        look_point.point.y = 0.0
        look_point.point.z = 0.0

        self.looker.target = look_point
        self.start()
Esempio n. 35
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.')
Esempio n. 36
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.')
Esempio n. 37
0
    def run(self, pose):
        rospy.logdebug("Action {}: Looking at point: {}".format(self.name, pose))

        # Parse out the pose
        parsed_pose = self._parse_pose(pose)
        if parsed_pose is None:
            rospy.logerr("Action {}: FAIL. Unknown Format: {}".format(self.name, pose))
            raise KeyError(self.name, "Unknown Format", pose)

        # Create and send the goal pose
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = parsed_pose['frame']
        goal.target.point.x = parsed_pose['x']
        goal.target.point.y = parsed_pose['y']
        goal.target.point.z = parsed_pose['z']
        goal.min_duration = rospy.Duration(self._duration)
        self._look_client.send_goal(goal)
        self.notify_action_send_goal(LookAction.HEAD_ACTION_SERVER, goal)

        # Yield an empty dict while we're executing
        while self._look_client.get_state() in AbstractStep.RUNNING_GOAL_STATES:
            yield self.set_running()

        # Wait for a result and yield based on how we exited
        status = self._look_client.get_state()
        self._look_client.wait_for_result()
        result = self._look_client.get_result()
        self.notify_action_recv_result(LookAction.HEAD_ACTION_SERVER, status, result)

        if status == GoalStatus.SUCCEEDED:
            yield self.set_succeeded()
        elif status == GoalStatus.PREEMPTED:
            yield self.set_preempted(
                action=self.name,
                status=status,
                goal=goal,
                result=result
            )
        else:
            yield self.set_aborted(
                action=self.name,
                status=status,
                goal=goal,
                result=result
            )
Esempio n. 38
0
    def __init__(self):
        self._default_focus_point = Point(1, 0, 1.05)
        self._down_focus_point = Point(0.5, 0, 0.5)
        self._target_focus_point = Point(1, 0, 1.05)
        self._current_focus_point = Point(1, 0, 1.05)

        self._current_gaze_action = GazeGoal.LOOK_FORWARD
        self._prev_gaze_action = self._current_gaze_action
        self._prev_target_focus_point = array(self._default_focus_point)

        # Some constants
        self._no_interrupt = [GazeGoal.NOD,
                               GazeGoal.SHAKE, GazeGoal.LOOK_DOWN]
        self._nod_positions = [Point(1, 0, 0.70), Point(1, 0, 1.20)]
        self._shake_positions = [Point(1, 0.2, 1.05), Point(1, -0.2, 1.05)]
        self._n_nods = 5
        self._n_shakes = 5

        self._nod_counter = 5
        self._shake_counter = 5
        self._face_pos = None
        self._glance_counter = 0

        self.gaze_goal_strs = {
            GazeGoal.LOOK_FORWARD: 'LOOK_FORWARD',
            GazeGoal.FOLLOW_EE: 'FOLLOW_EE',
            GazeGoal.GLANCE_EE: 'GLANCE_EE',
            GazeGoal.NOD: 'NOD',
            GazeGoal.SHAKE: 'SHAKE',
            GazeGoal.FOLLOW_FACE: 'FOLLOW_FACE',
            GazeGoal.LOOK_AT_POINT: 'LOOK_AT_POINT',
            GazeGoal.LOOK_DOWN: 'LOOK_DOWN',
            GazeGoal.NONE: 'NONE',
        }

        ## Action client for sending commands to the head.
        self._head_action_client = SimpleActionClient(
            '/head_controller/point_head', PointHeadAction)
        self._head_action_client.wait_for_server()
        rospy.loginfo('Head action client has responded.')

        self._head_goal = PointHeadGoal()
        self._head_goal.target.header.frame_id = 'base_link'
        self._head_goal.min_duration = rospy.Duration(1.0)
        self._head_goal.target.point = Point(1, 0, 1)

        ## Service client for arm states
        self._tf_listener = TransformListener()

        ## Server for gaze requested gaze actions
        self._gaze_action_server = SimpleActionServer(
            'gaze_action', GazeAction, self._execute_gaze_action, False)
        self._gaze_action_server.start()

        self._is_action_complete = True

        rospy.Service('get_current_gaze_goal', GetGazeGoal,
                      self._get_gaze_goal)
Esempio n. 39
0
 def loop(self):
     while not rospy.is_shutdown():
         # Reset head if no face found in 1 second
         if time.time() - self.lastFaceCallback > 1:
             self.lastHeadTarget = None
             # Reset head
             pos = PointStamped()
             pos.header.stamp = rospy.Time.now()
             pos.header.frame_id = "base_link"
             pos.point.x = 1
             pos.point.y = 0
             pos.point.z = 1.5
             goal = PointHeadGoal()
             goal.min_duration = rospy.Duration(0.5)
             goal.target = pos
             #self.client.cancel_all_goals()
             self.client.send_goal(goal)
         rospy.sleep(0.1)
Esempio n. 40
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!")
Esempio n. 41
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!")
 def __init__(self):
     self.client = actionlib.SimpleActionClient(
         "movo/head_controller/point_head_action", PointHeadAction)
     rospy.loginfo("Waiting for head_controller...")
     self.client.wait_for_server()
     self.pag = PointHeadGoal()
     self.pag.max_velocity = 1.0
     self.pag.pointing_axis.x = 1.0
     self.pag.pointing_frame = "/kinect2_link"
Esempio n. 43
0
    def look_at_bin(self):
        print("look at the bin")
        head_client = actionlib.SimpleActionClient(
            "head_controller/point_head", PointHeadAction)
        head_client.wait_for_server()
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = "base_link"
        goal.target.point.x = 0.7
        goal.target.point.y = 0
        goal.target.point.z = 0.4
        goal.min_duration = rospy.Duration(1.0)
        head_client.send_goal(goal)
        head_client.wait_for_result()

        rospy.loginfo("Wating for ar_pose_marker topic...")
        rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
        rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
Esempio n. 44
0
def look_at(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(0.5)
	client.send_goal(goal)
Esempio n. 45
0
def look_at(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(0.5)
    client.send_goal(goal)
Esempio n. 46
0
def move():
	rospy.init_node("move")

	move_group = MoveGroupInterface("arm_with_torso", "base_link")


	#planning scene setup
	planning_scene = PlanningSceneInterface("base_link")
	planning_scene.removeCollisionObject("my_front_ground")
	planning_scene.removeCollisionObject("my_back_ground")
	planning_scene.removeCollisionObject("my_right_ground")
	planning_scene.removeCollisionObject("my_left_ground")
	planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
	planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
	planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
	planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)


	#move head camera
	client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
	rospy.loginfo("Waiting for head_controller...")
	client.wait_for_server()

	goal = PointHeadGoal()
	goal.target.header.stamp = rospy.Time.now()
	goal.target.header.frame_id = "base_link"
	goal.target.point.x = 1.2
	goal.target.point.y = 0.0
	goal.target.point.z = 0.0
	goal.min_duration = rospy.Duration(1.0)
	client.send_goal(goal)
	client.wait_for_result()

	#arm setup
	move_group = MoveGroupInterface("arm", "base_link")
	#set arm initial position
	joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
	pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
	while not rospy.is_shutdown():
		result = move_group.moveToJointPosition(joints, pose, 0.02)
		if result.error_code.val == MoveItErrorCodes.SUCCESS:
			return
Esempio n. 47
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.')
Esempio n. 48
0
 def execute(self, userdata):
     
     if userdata.point_to_look == None and self.point_to_look == None:
         rospy.logerr("No point to look at! Error at SM Look_to_point")
         return 'aborted'
     print str(self.direction)
     
     self.point_to_look = self.point_to_look if self.point_to_look else userdata.point_to_look
     
     phg = PointHeadGoal()
     phg.min_duration = rospy.Duration(self.min_duration) # adapt for as far as the detection is??
     
     phg.target.header.frame_id = self.frame_id if self.frame_id else self.point_to_look.header.frame_id
     
     phg.target.header.stamp = rospy.Time.now()
     
     phg.target.point.x = self.point_to_look.point.x
     phg.target.point.y = self.point_to_look.point.y
     phg.target.point.z = self.point_to_look.point.z
     
     phg.pointing_axis.x = 1.0
     phg.pointing_frame = 'head_mount_xtion_rgb_frame'
     
     if self.direction!="" :
         print "I HAVE A NEW ORDER"
         phg.target.point.z=UP
         if self.direction =="left" :
             phg.target.point.y = 1
         elif self.direction=="right":
             phg.target.point.y = -1
         elif self.direction=="front":
             phg.target.point.y = 0
         elif self.direction=="up":
             phg.target.point.y = 0
             phg.target.point.z=1.6
     
     
     
     userdata.point_head_goal = phg
     print phg
     return 'succeeded'
 def head_action(self, x, y, z, wait = False):
     name_space = '/head_traj_controller/point_head_action'
     head_client = SimpleActionClient(name_space, PointHeadAction)
     head_goal = PointHeadGoal()
     head_goal.target.header.frame_id = 'base_link'
     head_goal.min_duration = rospy.Duration(1.0)
     head_goal.target.point = Point(x, y, z)
     head_client.send_goal(head_goal)
     if wait:
         # wait for the head movement to finish before we try to detect and pickup an object
         finished_within_time = head_client.wait_for_result(rospy.Duration(5))
         # Check for success or failure
         if not finished_within_time:
             head_client.cancel_goal()
             rospy.loginfo("Timed out achieving head movement goal")
         else:
             state = head_client.get_state()
             if state == GoalStatus.SUCCEEDED:
                 rospy.loginfo("Head movement goal succeeded!")
                 rospy.loginfo("State:" + str(state))
             else:
               rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
Esempio n. 50
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()
Esempio n. 51
0
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('actionlib')
roslib.load_manifest('actionlib_msgs')
roslib.load_manifest('control_msgs')
roslib.load_manifest('geometry_msgs')

import rospy
from control_msgs.msg import PointHeadAction
from control_msgs.msg import PointHeadGoal
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from geometry_msgs.msg import Point

if __name__ == "__main__":
    rospy.init_node('head_test_node', anonymous=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 = 'base_link'
    head_goal.min_duration = rospy.Duration(1.0)
    # head_goal.target.point = Point(1, 0, 1) # look forward!
    head_goal.target.point = Point(1, 0, 0.5) # look forward!
    head_client.send_goal(head_goal)
    head_client.wait_for_result(rospy.Duration(10.0))
    if (head_client.get_state() != GoalStatus.SUCCEEDED):
        rospy.logwarn('Head action unsuccessful.')