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))
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)
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)
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)
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]))
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'
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
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.')
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()
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.')
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 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)
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()
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 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)
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
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()
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
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 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)
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()
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)
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")
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)
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()
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.')
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.')
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
)
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)
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)
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 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"
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)
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)
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)
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
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.')
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]))
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()
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.')
