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 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 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 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))
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 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 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 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 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 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 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 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 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 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 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(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... ')
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_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_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 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 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_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 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 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()
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 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()
def look_at(self, frame_id, x, y, z): """Moves the head to look at a point in space. Args: frame_id: The name of the frame in which x, y, and z are specified. x: The x value of the point to look at. y: The y value of the point to look at. z: The z value of the point to look at. """ # TODO: Create goal goal = PointHeadGoal() # t = JointTrajectory() # t.header.frame_id = frame_id pointStamped = PointStamped() point = Point() point.x = x point.y = y point.z = z # point.positions.append(y) # point.positions.append(z) pointStamped.point = point # goal.target.append(point) goal.target = pointStamped goal.target.header.frame_id = frame_id # t.points.append(point) # t.joint_names.append(PAN_JOINT) # t.joint_names.append(TILT_JOINT) # TODO: Fill out the goal (we recommend setting min_duration to 1 second) goal.min_duration = rospy.Duration(secs=1, nsecs=0) # TODO: Send the goal # goal.trajectory = t self._look_client.send_goal(goal) # TODO: Wait for result self._look_client.wait_for_result()
def move(): rospy.init_node("move") move_group = MoveGroupInterface("arm_with_torso", "base_link") #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.5 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()
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 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.')