def handle_enable_leaning_forward(self, msg): self.enable_leaning_forward = msg.data if msg.data: self.motionProxy.setBreathEnabled('Legs', False) cmd_msg = JointAnglesWithSpeed() cmd_msg.header.stamp = rospy.Time.now() cmd_msg.joint_names.append('LHipPitch') cmd_msg.joint_angles.append(-9.0 * math.pi / 180.0) cmd_msg.joint_names.append('RHipPitch') cmd_msg.joint_angles.append(-9.0 * math.pi / 180.0) cmd_msg.speed = 0.06 cmd_msg.relative = 0 self.pub_joint_cmd.publish(cmd_msg) else: cmd_msg = JointAnglesWithSpeed() cmd_msg.header.stamp = rospy.Time.now() cmd_msg.joint_names.append('LHipPitch') cmd_msg.joint_angles.append(0.0 * math.pi / 180.0) cmd_msg.joint_names.append('RHipPitch') cmd_msg.joint_angles.append(0.0 * math.pi / 180.0) cmd_msg.speed = 0.06 cmd_msg.relative = 0 self.motionProxy.setBreathEnabled('Legs', True)
def handle_idle_status(self, msg): if msg.enabled: self.motionProxy.setBreathEnabled('Body', True) self.motionProxy.setBreathEnabled('Head', False) if self.enable_leaning_forward: self.motionProxy.setBreathEnabled('Legs', False) self.motionProxy.setBreathConfig([['Bpm', 12], ['Amplitude', 0.8]]) if self.enable_leaning_forward: rospy.sleep(0.6) cmd_msg = JointAnglesWithSpeed() cmd_msg.header.stamp = rospy.Time.now() cmd_msg.joint_names.append('LHipPitch') cmd_msg.joint_angles.append(-9.0 * math.pi / 180.0) cmd_msg.joint_names.append('RHipPitch') cmd_msg.joint_angles.append(-9.0 * math.pi / 180.0) cmd_msg.speed = 0.1 cmd_msg.relative = 0 self.pub_joint_cmd.publish(cmd_msg) rospy.sleep(0.2) else: self.postureProxy.goToPosture("Stand", 0.2) self.motionProxy.setBreathEnabled('Body', False) self.motionProxy.setBreathEnabled('Head', False) if self.enable_leaning_forward: self.motionProxy.setBreathEnabled('Legs', False)
def larm(): names = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll'] pub = rospy.Publisher('/joint_angles', JointAnglesWithSpeed, queue_size=1) rospy.init_node('nao_larm', anonymous=True) rate = rospy.Rate(10) # 10hz with open(filename, 'rU') as csvfile: csvreader = csv.reader(csvfile) for row in csvreader: cols = [] angles = [] for col in row: cols.append(col) for i in range(2, 6): angles.append(float(cols[i])) print names, angles msg = JointAnglesWithSpeed() msg.joint_names = names msg.joint_angles = angles msg.speed = 1.0 msg.header.frame_id = 'Fixed Frame' pub.publish(msg) rate.sleep()
def publish_joints(self, names, angles): m = JointAnglesWithSpeed() m.header.stamp = rospy.Time.now() m.joint_names = names m.joint_angles = angles m.relative = 0 m.speed = 1 self.__pub_joints.publish(m)
def callback_waving(self, location): ''' this function takes the information that waving has been detected and decides whether Pepper has to turn his head (when the waving is not in the center of view) or if he publishes the currect head ''' a,b=location.data.split(" ") self.counter2 = 0 locationlist=[int(a),int(b)] if locationlist[0] < 0 and locationlist[1] < 0: newpos= abs(locationlist[0]-locationlist[1]) if newpos > 10: newpos = 10 msg = JointAnglesWithSpeed() msg.joint_names=["HeadYaw"] msg.joint_angles=[newpos*almath.TO_RAD] msg.speed=0.05 msg.relative=1 self.joint_angles_pub.publish(msg) elif locationlist[0] > 0 and locationlist[1] > 0: newpos= abs(locationlist[0]-locationlist[1]) if newpos > 10: newpos = 10 msg = JointAnglesWithSpeed() msg.joint_names=["HeadYaw"] msg.joint_angles=[-newpos*almath.TO_RAD] msg.speed=0.05 msg.relative=1 self.joint_angles_pub.publish(msg) else: self.counter = self.counter +1 #this is supposed to tell you that a waving person has been found in the direction he's looking into #the numbers at the end of the sent String are supposed to tell you the current angle of Peppers head, not the direction he's supposed to turn to #it uses a type of costum message that hasn't been defined yet msg = PositionCommand() msg.command= "go" msg.location= "waving " + str(self.position) self.go_to_pub.publish(msg)
def set_joint_angles(self, joint_name, angle_value): joint_angles_to_set = JointAnglesWithSpeed() joint_angles_to_set.joint_names.append(joint_name) # each joint has a specific name, look into the joint_state topic or google joint_angles_to_set.joint_angles.append(angle_value) # the joint values have to be in the same order as the names!! joint_angles_to_set.relative = False # if true you can increment positions joint_angles_to_set.speed = 0.1 # keep this low if you can self.jointPub.publish(joint_angles_to_set)
def stand(self): print "STANDING" self.__call_service( "/naoqi_driver/robot_posture/go_to_posture", GoToPosture, GoToPostureRequest(GoToPostureRequest.STAND_INIT, 0.5)) j = JointAnglesWithSpeed() j.joint_names = ['HeadYaw', 'HeadPitch'] j.joint_angles = [0., -.5] j.speed = .05 self.joints.publish(j)
def Elbow_yaw_R_moveto(self, arm_angle): # controls right elbow movement joint_angles_to_set = JointAnglesWithSpeed() joint_angles_to_set.joint_names.append( "RElbowYaw" ) # each joint has a specific name, look into the joint_state topic or google joint_angles_to_set.joint_angles.append( arm_angle ) # the joint values have to be in the same order as the names!! joint_angles_to_set.relative = False # if true you can increment positions joint_angles_to_set.speed = 0.1 # keep this low if you can self.jointPub.publish(joint_angles_to_set)
def Shoulder_roll_L_moveto(self, angle): # controls left shoulder movement joint_angles_to_set = JointAnglesWithSpeed() joint_angles_to_set.joint_names.append( "LShoulderRoll" ) # each joint has a specific name, look into the joint_state topic or google joint_angles_to_set.joint_angles.append( angle ) # the joint values have to be in the same order as the names!! joint_angles_to_set.relative = False # if true you can increment positions joint_angles_to_set.speed = 0.1 # keep this low if you can self.jointPub.publish(joint_angles_to_set)
def set_head_angles(self, head_angle): ## controls head movement joint_angles_to_set = JointAnglesWithSpeed() joint_angles_to_set.joint_names.append( "HeadYaw" ) # each joint has a specific name, look into the joint_state topic or google joint_angles_to_set.joint_angles.append( head_angle ) # the joint values have to be in the same order as the names!! joint_angles_to_set.relative = False # if true you can increment positions joint_angles_to_set.speed = 0.1 # keep this low if you can self.jointPub.publish(joint_angles_to_set) joint_angles_to_set.joint_names.append("HeadPitch") joint_angles_to_set.joint_angles.append(head_angle) self.jointPub.publish(joint_angles_to_set)
def handle_gaze_controller(self, event): # try: with self.lock: try: aaa = PointStamped() aaa.header.stamp = rospy.Time() aaa.header.frame_id = self.target.target_point.header.frame_id aaa.point.x = self.target.target_point.point.x aaa.point.y = self.target.target_point.point.y aaa.point.z = self.target.target_point.point.z point_transformed = self.tf_buf.transform(aaa, 'gaze') except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): e = sys.exc_info()[0] rospy.logdebug(e) rospy.logwarn("[%s] Can't tranform from gaze to target.[ %s - %s ]"%(rospy.get_name(), 'gaze', self.target.target_point.header.frame_id)) return pan_angle = math.atan2(point_transformed.point.y, point_transformed.point.x) tilt_angle = math.atan2(point_transformed.point.z, point_transformed.point.x) delta_pan_angle = pan_angle delta_tilt_angle = -tilt_angle cmd_pan_angle = 0.0 cmd_tilt_angle = 0.0 if abs(delta_pan_angle) < (2.0 * math.pi / 180.0): cmd_pan_angle = 0.0 else: cmd_pan_angle = 0.3 * delta_pan_angle if abs(delta_tilt_angle) < (2.0 * math.pi / 180.0): cmd_tilt_angle = 0.0 else: cmd_tilt_angle = 0.3 * delta_tilt_angle cmd_msg = JointAnglesWithSpeed() cmd_msg.header.stamp = rospy.Time.now() cmd_msg.joint_names.append('HeadYaw') cmd_msg.joint_angles.append(cmd_pan_angle) cmd_msg.joint_names.append('HeadPitch') cmd_msg.joint_angles.append(cmd_tilt_angle) cmd_msg.speed = self.target.max_speed cmd_msg.relative = 1 self.pub_gaze_cmd.publish(cmd_msg)
def idle(self): j = JointAnglesWithSpeed() j.joint_names = ['HeadYaw', 'HeadPitch'] j.joint_angles = [.5, -.5] j.speed = .05 pub = rospy.Publisher("/joint_angles", JointAnglesWithSpeed, queue_size=10) while not rospy.is_shutdown() and self.is_idle: try: rospy.wait_for_message("/naoqi_driver_node/people_detected", PersonDetectedArray, timeout=5.) except rospy.ROSException, rospy.ROSInterruptException: # j.joint_angles[0] *= -1. # j.joint_angles[1] = np.random.rand()-.5 j.joint_angles = [ np.random.rand() - .5, -(np.random.rand() * .6) ] pub.publish(j)
def track_bounding_box(self, polygon): self.hunt_initiated = True # Set the timeout counter to 2 seconds self.lost_object_counter = 20 # Velocities message initialization joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True # Get center of detected object and calculate the head turns target_x = polygon.points[0].x + 0.5 * polygon.points[1].x target_y = polygon.points[0].y + 0.5 * polygon.points[1].y sub_x = target_x - 320 / 2.0 sub_y = target_y - 240 / 2.0 var_x = (sub_x / 160.0) var_y = (sub_y / 120.0) joint.joint_angles.append(-var_x * 0.05) joint.joint_angles.append(var_y * 0.05) ans = self.rh.humanoid_motion.getJointAngles(['HeadYaw', 'HeadPitch']) head_pitch = ans['angles'][1] head_yaw = ans['angles'][0] # Get the sonar measurements sonars = self.rh.sensors.getSonarsMeasurements() # Check if NAO is close to an obstacle if sonars['sonars']['front_left'] <= 0.3 or sonars['sonars']['front_right'] <= 0.3: self.lock_motion = True rospy.loginfo("Locked due to sonars") # Check if NAOs head looks way too down or up elif head_pitch >= 0.4 or head_pitch <= -0.4: self.lock_motion = True rospy.loginfo("Locked due to head pitch") # Else approach the object elif self.lock_motion is False: self.theta_vel = head_yaw * 0.1 if -0.2 < head_yaw < 0.2: print "Approaching" self.x_vel = 0.5 self.y_vel = 0.0 else: self.x_vel = 0.0 self.y_vel = 0.0 print "Centering" self.joint_pub.publish(joint) # Check the battery levels batt = self.rh.sensors.getBatteryLevels() battery = batt['levels'][0] if battery < 25: self.rh.audio.setVolume(100) self.rh.audio.speak("My battery is low") self.predator_sub.unregister() self.rh.humanoid_motion.goToPosture("Sit", 0.7) self.rh.motion.disableMotors() sys.exit(1)
def callback_camera(self, ros_data): if self.tracking == 0: # When te ball is detected, save the desired frame and process the image to obtain de HSV values self.cv_image = self.bridge.imgmsg_to_cv2( ros_data, desired_encoding="passthrough") elif self.tracking == 1: # Use the bridge to convert the ROS Image message to OpenCV message cv_image = self.bridge.imgmsg_to_cv2( ros_data, desired_encoding="passthrough") # convert the cv_image to the HSV color space hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) # construct a mask for the color "colour", then perform a series of dilations and erosions to remove any small blobs left in the mask mask = cv2.inRange(hsv, self.colourLower, self.colourUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) # find contours in the mask and initialize the current (x, y) center of the ball cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] center = None # only proceed if at least one contour was found if len(cnts) > 0: # find the largest contour in the mask, then use it to compute the minimum enclosing circle and centroid c = max(cnts, key=cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) # only proceed if the radius meets a minimum size if radius > 10: # draw the circle and centroid on the cv_image cv2.circle(cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(cv_image, center, 5, (0, 0, 255), -1) # Use the bridge to convert the OpenCV message to ROS Image message. Publish the modified image on a new topic ros_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.publisher.publish(ros_image) # Work out the head movement required to keep the ball in the image center # Velocities message initialization joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True #True # Get center of detected object and calculate the head turns target_x = center[0] # centroid coordenate x target_y = center[1] # centroid coordenate y # Image center -> (cv_image.shape[1]/2, cv_image.shape[0]/2)=(320/2,240/2) (x,y) coordenates sub_x = target_x - cv_image.shape[1] / 2 sub_y = target_y - cv_image.shape[0] / 2 var_x = (sub_x / float(cv_image.shape[1] / 2)) var_y = (sub_y / float(cv_image.shape[0] / 2)) # Check the Head position before moving it to restrict its movement and prevent it from crashing with the shoulders # Predict the new Y relative position posicion, the one to be restricted to avoid crashes new_position_y = self.positions[1] + var_y * 0.10 if (new_position_y <= -0.45): var_y = 0 elif (new_position_y >= 0.3): var_y = 0 joint.joint_angles.append(-var_x * 0.25) joint.joint_angles.append(var_y * 0.10) self.joint_pub.publish(joint) # Movement towards the ball (walk) walk = Twist() print radius print self.positions[0] # HeadYaw <--> if radius < 50: # Walk forward if the ball is far. When radius = 50, Nao is aprox. 25cm from the ball # Walk if self.positions[0] < (-0.3): walk.linear.x = 0.3 walk.linear.y = -0.3 elif (self.positions[0] >= -0.3) and (self.positions[0] < 0.3): walk.linear.x = 0.3 walk.linear.y = 0.0 elif self.positions[0] >= 0.3: walk.linear.x = 0.3 walk.linear.y = 0.3 walk.linear.z = 0.0 walk.angular.x = 0.0 walk.angular.y = 0.0 walk.angular.z = 0.0 else: # Stop walk.linear.x = 0.0 walk.linear.y = 0.0 walk.linear.z = 0.0 walk.angular.x = 0.0 walk.angular.y = 0.0 walk.angular.z = 0.0 self.walk_pub.publish(walk)
def callback_tactile(self, data): rospy.loginfo("I heard %u %u" % (data.button, data.state)) if data.button == 1 and data.state == 1: joint_pub = rospy.Publisher('/joint_angles', JointAnglesWithSpeed, queue_size=10) rospy.loginfo("Pulsado %u , pasamos" % (data.button)) joint = JointAnglesWithSpeed() joint.joint_names.append("LShoulderRoll") joint.joint_angles.append(-0.3) joint.joint_names.append("LShoulderPitch") joint.joint_angles.append(-0.2) joint.joint_names.append("LElbowYaw") joint.joint_angles.append(-1) joint.joint_names.append("LElbowRoll") joint.joint_angles.append(0) joint.joint_names.append("LWristYaw") joint.joint_angles.append(-0.5) joint.joint_names.append("LHand") joint.joint_angles.append(0) joint.joint_names.append("RShoulderRoll") joint.joint_angles.append(0.3) joint.joint_names.append("RShoulderPitch") joint.joint_angles.append(-0.2) joint.joint_names.append("RElbowYaw") joint.joint_angles.append(1) joint.joint_names.append("RElbowRoll") joint.joint_angles.append(0) joint.joint_names.append("RWristYaw") joint.joint_angles.append(0.5) joint.joint_names.append("RHand") joint.joint_angles.append(0) joint.joint_names.append("HeadYaw") joint.joint_angles.append(0) joint.joint_names.append("HeadPitch") joint.joint_angles.append(0) joint.speed = 0.1 joint.relative = False joint_pub.publish(joint) # Deactivate tracking mode because the HSV range recongition is in process self.tracking = 0 print "Tracking deactivated" elif data.button == 2 and data.state == 1: # Save a frame from the camera to detect the ball colour output = self.cv_image.copy( ) # draw our detected circles without destroying the original image. gray = cv2.cvtColor( self.cv_image, cv2.COLOR_BGR2GRAY ) # the cv2.HoughCircles function requires an 8-bit, single channel image, so we'll go ahead and convert from the RGB color space to grayscale # detect circles in the image circles = cv2.HoughCircles(gray, cv2.cv.CV_HOUGH_GRADIENT, 1.2, 100, param2=80, minRadius=50, maxRadius=200) if circles == None: print 'No ball detected' else: print circles # ensure at least some circles were found if circles is not None: # convert the (x, y) coordinates and radius of the circles to integers allowing us to draw them on our output image circles = np.round(circles[0, :]).astype("int") # loop over the (x, y) coordinates and radius of the circles for (x, y, r) in circles: # draw the circle in the output image, then draw a rectangle # corresponding to the center of the circle cv2.circle(output, (x, y), r, (0, 255, 0), 4) cv2.rectangle(output, (x - 45, y - 45), (x + 45, y + 45), (0, 128, 255), -1) hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2HSV) # Calculate the HSV medium values inside a rectangle near to the ball center. s = [0, 0, 0] for i in range(y - 45, y + 46): for j in range(x - 45, x + 46): s = s + hsv[i, j] s = s / ((45 + 46) * (45 + 46)) print s # Calculate the upper and lower limits from the HSV medium values if s[1] < 100: # Saturation<100 if (s[0] - 10) < 0: self.colourLower = (0, 40, 40) else: self.colourLower = (s[0] - 10, 40, 40) else: if (s[0] - 10) < 0: self.colourLower = (0, 100, 100) else: self.colourLower = (s[0] - 10, 100, 100) self.colourUpper = (s[0] + 10, 255, 255) print self.colourUpper, self.colourLower elif data.button == 3 and data.state == 1: joint_pub = rospy.Publisher('/joint_angles', JointAnglesWithSpeed, queue_size=10) rospy.loginfo("Pulsado %u , pasamos" % (data.button)) joint = JointAnglesWithSpeed() joint.joint_names.append("LShoulderRoll") joint.joint_angles.append(0) joint.joint_names.append("LShoulderPitch") joint.joint_angles.append(1.3) joint.joint_names.append("LElbowYaw") joint.joint_angles.append(0) joint.joint_names.append("LElbowRoll") joint.joint_angles.append(0) joint.joint_names.append("LWristYaw") joint.joint_angles.append(0) joint.joint_names.append("LHand") joint.joint_angles.append(0) joint.joint_names.append("RShoulderRoll") joint.joint_angles.append(0) joint.joint_names.append("RShoulderPitch") joint.joint_angles.append(1.3) joint.joint_names.append("RElbowYaw") joint.joint_angles.append(0) joint.joint_names.append("RElbowRoll") joint.joint_angles.append(0) joint.joint_names.append("RWristYaw") joint.joint_angles.append(0) joint.joint_names.append("RHand") joint.joint_angles.append(0) joint.speed = 0.1 joint.relative = False joint_pub.publish(joint) # colour range detected. Activate tracking mode self.tracking = 1 print "Tracking ON"
def callback_camera(self, ros_data): # When te ball is detected, save the desired frame and process the image to obtain de HSV values if self.tracking == 0: self.cv_image = self.bridge.imgmsg_to_cv2(ros_data, desired_encoding="passthrough") elif self.tracking == 1: # Use the bridge to convert the ROS Image message to OpenCV message cv_image = self.bridge.imgmsg_to_cv2(ros_data, desired_encoding="passthrough") # convert the cv_image to the HSV color space hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) # construct a mask for the color "colour", then perform a series of dilations and erosions to remove any small blobs left in the mask mask = cv2.inRange(hsv, self.colourLower, self.colourUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) # find contours in the mask and initialize the current (x, y) center of the ball cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] center = None # only proceed if at least one contour was found if len(cnts) > 0: # find the largest contour in the mask, then use it to compute the minimum enclosing circle and centroid c = max(cnts, key=cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) # only proceed if the radius meets a minimum size if radius > 10: # draw the circle and centroid on the cv_image cv2.circle(cv_image, (int(x), int(y)), int(radius),(0, 255, 255), 2) cv2.circle(cv_image, center, 5, (0, 0, 255), -1) # Use the bridge to convert the OpenCV message to ROS Image message. Publish the modified image on a new topic ros_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.publisher.publish(ros_image) # Work out the head movement required to keep the ball in the image center # Velocities message initialization joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True#True # Get center of detected object and calculate the head turns target_x =center[0] # centroid coordenate x target_y=center[1] # centroid coordenate y # Image center -> (cv_image.shape[1]/2, cv_image.shape[0]/2)=(320/2,240/2) (x,y) coordenates sub_x = target_x - cv_image.shape[1]/2 sub_y = target_y - cv_image.shape[0]/2 var_x = (sub_x / float(cv_image.shape[1]/2)) var_y = (sub_y / float(cv_image.shape[0]/2)) # Check the Head position before moving it to restrict its movement and prevent it from crashing with the shoulders # Predict the new Y relative position posicion, the one to be restricted to avoid crashes new_position_y=self.positions[1]+var_y*0.10 if (new_position_y <= -0.45 ): var_y=0 elif (new_position_y >= 0.3): var_y=0 joint.joint_angles.append(-var_x*0.25) joint.joint_angles.append(var_y*0.10) self.joint_pub.publish(joint) # Movement towards the ball (walk) walk=Twist() print " Radius is (px): ", radius print "HeadPitch: ", self.positions[1] print "HeadYaw: ", self.positions[0] # HeadYaw <--> if radius < 58: # Walk forward if the ball is far. When radius = 50, Nao is aprox. 25cm from the ball # Walk if self.positions[0] < (-0.3): walk.linear.x=0.2 walk.angular.z=-0.25 elif (self.positions[0] >= -0.3) and (self.positions[0] < 0.3): walk.linear.x=0.3 walk.angular.z=0.0 elif self.positions[0] >= 0.3: walk.linear.x=0.2 walk.angular.z=0.25 walk.linear.y=0.0 walk.linear.z=0.0 walk.angular.x=0.0 walk.angular.y=0.0 self.walk_pub.publish(walk) elif radius > 68: # If it is too close, walk backwards walk.linear.x=-0.2 walk.linear.y=0.0 walk.linear.z=0.0 walk.angular.x=0.0 walk.angular.y=0.0 walk.angular.z=0.0 self.walk_pub.publish(walk) print "Walk backwards" else: CabezaCentrada=False PelotaCentro=False # Check if the ball is in front of Nao, if not: Nao walks to the corresponding side # Check it the head is aligned with the body, and that is not turned if self.positions[0] < (-0.3) or self.positions[0] > 0.3: # Use a proportional controller to move with more precision walk.linear.y=0.0 e=0-self.positions[0] walk.angular.z=e*(-0.5) elif self.positions[0] < (-0.05): # HeadYaw <--> walk.linear.y=-0.3 walk.angular.z=-0.0 elif self.positions[0] > 0.05: walk.linear.y=0.3 walk.angular.z=0.0 else: # Stop CabezaCentrada=True walk.linear.y=0.0 walk.angular.z=0.0 print " Head centered " walk.linear.x=0.0 walk.linear.z=0.0 walk.angular.x=0.0 walk.angular.y=0.0 self.walk_pub.publish(walk) rospy.sleep(0.3) # Wait some time for the robot to become steady # if center[0]< 152 or center [0]> 168: # Check if the ball is in the image center looking at its X coordinate e=160-center[0] walk.linear.y=e*(-0.005) walk.angular.z=-0.0 else: # Parar PelotaCentro=True walk.linear.y=0.0 walk.angular.z=0.0 print " Ball centered " walk.linear.x=0.0 walk.linear.z=0.0 walk.angular.x=0.0 walk.angular.y=0.0 self.walk_pub.publish(walk) print " Center is: ", center print "Pelota Centro: ", PelotaCentro print "Cabeza Centrada: ", CabezaCentrada if PelotaCentro and CabezaCentrada: self.image_sub.unregister() # Subscribe to another callback self.image_sub = rospy.Subscriber("/nao_robot/camera/top/camera/image_raw", Image, self.callback_camera2)
def callback_camera2(self, ros_data): print "callback camera 2: centrado de pelota en la imagen" # Use the bridge to convert the ROS Image message to OpenCV message cv_image = self.bridge.imgmsg_to_cv2(ros_data, desired_encoding="passthrough") # convert the cv_image to the HSV color space hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) # construct a mask for the color "colour", then perform a series of dilations and erosions to remove any small blobs left in the mask mask = cv2.inRange(hsv, self.colourLower, self.colourUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) # find contours in the mask and initialize the current (x, y) center of the ball cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] center = None # only proceed if at least one contour was found if len(cnts) > 0: # find the largest contour in the mask, then use it to compute the minimum enclosing circle and centroid c = max(cnts, key=cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) # only proceed if the radius meets a minimum size if radius > 10: # draw the circle and centroid on the cv_image cv2.circle(cv_image, (int(x), int(y)), int(radius),(0, 255, 255), 2) cv2.circle(cv_image, center, 5, (0, 0, 255), -1) # Use the bridge to convert the OpenCV message to ROS Image message. Publish the modified image on a new topic ros_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.publisher.publish(ros_image) if center[0]<155 or center[0]>165: # Check X coordinate self.contador=0 # Initialize counter # Work out the head movement required to keep the ball in the image center # Velocities message initialization joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True # Get center of detected object and calculate the head turns target_x =center[0] # centroid coordenate x target_y=center[1] # centroid coordenate y # Image center -> (cv_image.shape[1]/2, cv_image.shape[0]/2)=(320/2,240/2) (x,y) coordenates sub_x = target_x - cv_image.shape[1]/2 sub_y = target_y - cv_image.shape[0]/2 var_x = (sub_x / float(cv_image.shape[1]/2)) var_y = (sub_y / float(cv_image.shape[0]/2)) # Check the Head position before moving it to restrict its movement and prevent it from crashing with the shoulders # Predict the new Y relative position posicion, the one to be restricted to avoid crashes new_position_y=self.positions[1]+var_y*0.15 if (new_position_y <= -0.45 ): var_y=0 elif (new_position_y >= 0.3): var_y=0 joint.joint_angles.append(-var_x*0.10) #0.25 joint.joint_angles.append(var_y*0.15) self.joint_pub.publish(joint) else: print "Centrado respecto a X" if center[1]<115 or center[1]>125: # Check Y coordinate self.contador=0 # Velocities message initialization joint = JointAnglesWithSpeed() #joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True#True if center[1]<115: var_y=-0.01 elif center[1]>125: var_y=0.01 joint.joint_angles.append(var_y) self.joint_pub.publish(joint) else: # Counter to ensure the robot is centeredr and it was nota coincidence self.contador=self.contador + 1 print "Centrado respecto a Y" print "Contador: ", self.contador if self.contador > 10: self.image_sub.unregister() print "Centrado y salimos" print "HeadPitch: ", self.positions[1] # HeadPitch print "HeadYaw: ", self.positions[0] # HeadYaw <--> print " Center is: ", center # Obtain position Dreal=0.08 # Real Ball diameter (meters) pixelToMeter=Dreal/(radius*2) Yrobot=(cv_image.shape[1]/2 - center[0])*pixelToMeter radiusToMeters=0.25*55 # Distance: 0.25m -> radius = 55px Xrobot=radiusToMeters/radius # Calculate Z coordinate with the equation that connects it with HeadPitch Zrobot=(-25.866*self.positions[1]+23.843)/100 # Divide by 100 to convert centimeters to meters # Create point point = Point() point.x = Xrobot point.y = Yrobot point.z = Zrobot self.pointPub.publish(point) print "Punto publicado: ", point print " El radio son (px): ", radius print "HeadPitch: ", self.positions[1] print "HeadYaw: ", self.positions[0] # HeadYaw <--> print " Center is: ", center
message_arrived=False rospy.init_node('talker6') rospy.Subscriber('/camshift/track_box',RotatedRectStamped,talker6) rospy.Subscriber('/pepper_robot/head_touch',HeadTouch,headtouch_cb) pub=rospy.Publisher('/pepper_robot/pose/joint_angles',JointAnglesWithSpeed,queue_size=1) #pub=rospy.Publisher('/pepper_robot/pose/joint_angles',JointAnglesWithSpeed,queue_size=1) rospy.sleep(1) #omajinai while not rospy.is_shutdown(): hello=JointAnglesWithSpeed(); #if message_arrived: if head_touch_message_arrived and head_touch_message_arrived.button==1 and head_touch_message_arrived.state==1: print("message_arrived") if True:#(message_arrived.rect.size.width*message_arrived.rect.size.height>4000): hello.joint_names.append("LShoulderPitch") hello.joint_names.append("RShoulderPitch") hello.speed=0.05 #hello.relative=0 hello.joint_angles=[-0.05,-0.05] print(message_arrived) message_arrived=False pub.publish(hello); rospy.sleep(1) #sys.exit(0) except rospy.ROSInterruptException: pass
def track_bounding_box(self, polygon): self.hunt_initiated = True # Set the timeout counter to 2 seconds self.lost_object_counter = 20 # Velocities message initialization joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True # Get center of detected object and calculate the head turns target_x = polygon.points[0].x + 0.5 * polygon.points[1].x target_y = polygon.points[0].y + 0.5 * polygon.points[1].y sub_x = target_x - 320 / 2.0 sub_y = target_y - 240 / 2.0 var_x = (sub_x / 160.0) var_y = (sub_y / 120.0) joint.joint_angles.append(-var_x * 0.05) joint.joint_angles.append(var_y * 0.05) ans = self.rh.humanoid_motion.getJointAngles(['HeadYaw', 'HeadPitch']) head_pitch = ans['angles'][1] head_yaw = ans['angles'][0] # Get the sonar measurements sonars = self.rh.sensors.getSonarsMeasurements() # Check if NAO is close to an obstacle if sonars['sonars']['front_left'] <= 0.3 or sonars['sonars'][ 'front_right'] <= 0.3: self.lock_motion = True rospy.loginfo("Locked due to sonars") # Check if NAOs head looks way too down or up elif head_pitch >= 0.4 or head_pitch <= -0.4: self.lock_motion = True rospy.loginfo("Locked due to head pitch") # Else approach the object elif self.lock_motion is False: self.theta_vel = head_yaw * 0.1 if -0.2 < head_yaw < 0.2: print "Approaching" self.x_vel = 0.5 self.y_vel = 0.0 else: self.x_vel = 0.0 self.y_vel = 0.0 print "Centering" self.joint_pub.publish(joint) # Check the battery levels batt = self.rh.sensors.getBatteryLevels() battery = batt['levels'][0] if battery < 25: self.rh.audio.setVolume(100) self.rh.audio.speak("My battery is low") self.predator_sub.unregister() self.rh.humanoid_motion.goToPosture("Sit", 0.7) self.rh.motion.disableMotors() sys.exit(1)
def callback_camera(self, ros_data): if self.tracking == 0: self.cv_image = self.bridge.imgmsg_to_cv2( ros_data, desired_encoding="passthrough") elif self.tracking == 1: # Use the bridge to convert the ROS Image message to OpenCV message cv_image = self.bridge.imgmsg_to_cv2( ros_data, desired_encoding="passthrough") # convert the cv_image to the HSV color space hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) # construct a mask for the color "colour", then perform a series of dilations and erosions to remove any small blobs left in the mask mask = cv2.inRange(hsv, self.colourLower, self.colourUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) # find contours in the mask and initialize the current (x, y) center of the ball cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] center = None # only proceed if at least one contour was found if len(cnts) > 0: # find the largest contour in the mask, then use it to compute the minimum enclosing circle and centroid c = max(cnts, key=cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) # only proceed if the radius meets a minimum size if radius > 10: # draw the circle and centroid on the cv_image cv2.circle(cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(cv_image, center, 5, (0, 0, 255), -1) # Conversion of the OpenCV format image to ROS format and publish it in a topic ros_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.publisher.publish(ros_image) # It calculates the movement of the head necessary to keep the ball in the center of the image # Velocities message initialization joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True #True # Get center of detected object and calculate the head turns target_x = center[0] # centroid coordenate x target_y = center[1] # centroid coordenate y # Image center -> (cv_image.shape[1]/2, cv_image.shape[0]/2)=(320/2,240/2) (x,y) coordenates sub_x = target_x - cv_image.shape[1] / 2 sub_y = target_y - cv_image.shape[0] / 2 var_x = (sub_x / float(cv_image.shape[1] / 2)) var_y = (sub_y / float(cv_image.shape[0] / 2)) # Check the position of the head before moving it to restrict movement and prevent it from clashing with the shoulders # Subscribe to the topic / joint_states and save the first two values (HeadYaw and HeadPitch) in reference variables to compare before performing the move # Determine the new relative position in Y, which is the one I restrict to avoid collisions new_position_y = self.positions[1] + var_y * 0.10 if (new_position_y <= -0.45): var_y = 0 elif (new_position_y >= 0.3): var_y = 0 joint.joint_angles.append(-var_x * 0.25) joint.joint_angles.append(var_y * 0.10) self.joint_pub.publish(joint) # Movement towards the ball (walking) walk = Twist() print " El radio son (px): ", radius print "HeadPitch: ", self.positions[1] print "HeadYaw: ", self.positions[0] # HeadYaw <--> if radius < 58: # Approach if far away. If I see radio 50 I am approx. 25cm # Walk if self.positions[0] < (-0.3): walk.linear.x = 0.2 walk.angular.z = -0.25 elif (self.positions[0] >= -0.3) and (self.positions[0] < 0.3): walk.linear.x = 0.3 walk.angular.z = 0.0 elif self.positions[0] >= 0.3: walk.linear.x = 0.2 walk.angular.z = 0.25 walk.linear.y = 0.0 walk.linear.z = 0.0 walk.angular.x = 0.0 walk.angular.y = 0.0 self.walk_pub.publish(walk) elif radius > 68: # When Nao is too close to the ball, it walks backwards walk.linear.x = -0.2 walk.linear.y = 0.0 walk.linear.z = 0.0 walk.angular.x = 0.0 walk.angular.y = 0.0 walk.angular.z = 0.0 self.walk_pub.publish(walk) print " Desplazo atras" else: CabezaCentrada = False PelotaCentro = False # Check if the ball is in front of the robot, otherwise it moves to the corresponding side # Check that the head is aligned with the body and that it is not turned to any side if self.positions[0] < (-0.3) or self.positions[0] > 0.3: walk.linear.y = 0.0 e = 0 - self.positions[0] walk.angular.z = e * (-0.5) elif self.positions[0] < (-0.05): # HeadYaw <--> walk.linear.y = -0.3 walk.angular.z = -0.0 elif self.positions[0] > 0.05: walk.linear.y = 0.3 walk.angular.z = 0.0 else: # Stop walking CabezaCentrada = True walk.linear.y = 0.0 walk.angular.z = 0.0 print " Cabeza centrada" walk.linear.x = 0.0 walk.linear.z = 0.0 walk.angular.x = 0.0 walk.angular.y = 0.0 self.walk_pub.publish(walk) rospy.sleep(0.3) # For the robot to stabilize if center[0] < 150 or center[ 0] > 170: # It is verified that the ball is seen in the center of the image through its X coordinate e = 160 - center[0] walk.linear.y = e * (-0.005) print "Regulador" walk.angular.z = -0.0 else: # Parar PelotaCentro = True walk.linear.y = 0.0 walk.angular.z = 0.0 print " Pelota en el centro" walk.linear.x = 0.0 walk.linear.z = 0.0 walk.angular.x = 0.0 walk.angular.y = 0.0 self.walk_pub.publish(walk) print " Center is: ", center print "Pelota Centro: ", PelotaCentro print "Cabeza Centrada: ", CabezaCentrada if PelotaCentro and CabezaCentrada: self.image_sub.unregister() # Subscribe to other callback self.image_sub = rospy.Subscriber( "/nao_robot/camera/top/camera/image_raw", Image, self.callback_camera2)
def main(self): ## Configuration parameters # Set the language of the speech recognition engine to Spanish: self.asrProxy.setLanguage("Spanish") # set the language of the synthesis engine to Spanish: self.ttsProxy.setLanguage("Spanish") # Adds the vocabulary vocabulary = [ "si", "no", "hola", "Nao", "rojo", "azul", "amarillo", "violeta" ] self.asrProxy.setVocabulary(vocabulary, False) # Or, if you want to enable word spotting: #self.asrProxy.setVocabulary(vocabulary, True) # Wake up robot self.motionProxy.wakeUp() fractionMaxSpeed = 0.5 # Go to posture stand self.postureProxy.goToPosture("StandInit", fractionMaxSpeed) # Introducting the game self.ttsProxy.say("Hola! Soy Nao.") # Ask the kid to choose one ball (blue, red, yellow) self.ttsProxy.say( "Elige una de las pelotas y cuando estés listo llámame.") # Start sound tracker while the kid is not ready. When the kid says something or claps his hands, Nao turn into his location. targetName = "Sound" distance = 1 confidence = 0.4 parameters = [distance, confidence] self.trackerProxy.registerTarget(targetName, parameters) mode = "Move" self.trackerProxy.setMode(mode) # Start tracker. self.trackerProxy.track(targetName) while True: time.sleep(1) TargetPosition = self.trackerProxy.getTargetPosition( 2) # Frame: 0-Torso, 1-World, 2-Robot print "Target Position: ", TargetPosition print len(TargetPosition) if len(TargetPosition) > 0: if TargetPosition[0] > 0 and abs( TargetPosition[1] ) < 0.1: # Exit from the loop when the kid voice is opposite the robot break self.trackerProxy.stopTracker() self.trackerProxy.unregisterAllTargets() self.ttsProxy.say("¡Al fin te encuentro!") # Go to posture stand self.postureProxy.goToPosture("StandInit", fractionMaxSpeed) self.ttsProxy.say("¿Me dejas ver la pelota?") repeate_recognition = 1 count = 0 while (repeate_recognition == 1): self.postureProxy.goToPosture("StandInit", fractionMaxSpeed) # Robot posture with hands opened to catch the ball #joint_pub = rospy.Publisher('/joint_angles', JointAnglesWithSpeed, queue_size=10) rospy.loginfo("Poniendo manos en posición de reconocimiento") joint = JointAnglesWithSpeed() joint.joint_names.append("LShoulderRoll") joint.joint_angles.append(-0.35) joint.joint_names.append("LShoulderPitch") joint.joint_angles.append(-0.30) joint.joint_names.append("LElbowYaw") joint.joint_angles.append(-1.00) joint.joint_names.append("LElbowRoll") joint.joint_angles.append(0.00) joint.joint_names.append("LWristYaw") joint.joint_angles.append(-1.50) joint.joint_names.append("LHand") joint.joint_angles.append(1) joint.joint_names.append("RShoulderRoll") joint.joint_angles.append(0.35) joint.joint_names.append("RShoulderPitch") joint.joint_angles.append(-0.30) joint.joint_names.append("RElbowYaw") joint.joint_angles.append(1.00) joint.joint_names.append("RElbowRoll") joint.joint_angles.append(0.00) joint.joint_names.append("RWristYaw") joint.joint_angles.append(1.50) joint.joint_names.append("RHand") joint.joint_angles.append(1) joint.joint_names.append("HeadYaw") joint.joint_angles.append(0.00) joint.joint_names.append("HeadPitch") joint.joint_angles.append(0.00) joint.speed = 0.1 joint.relative = False self.joint_pub.publish(joint) # Wait some time for the kid to put the ball in Nao's hands time.sleep(10) # 10 seconds # Take a frame with the ball to recognize the colour ball_detected = 0 while (ball_detected == 0): output = self.cv_image.copy( ) # Draw the detected circles without destroying the original image. gray = cv2.cvtColor( self.cv_image, cv2.COLOR_BGR2GRAY ) # The cv2.HoughCircles function requires an 8-bit, single channel image, so we'll go ahead and convert from the RGB color space to grayscale # Detect circles in the image circles = cv2.HoughCircles(gray, cv2.cv.CV_HOUGH_GRADIENT, 1.2, 100, param2=80, minRadius=50, maxRadius=200) if circles == None: print 'No se ha detectado la pelota' self.ttsProxy.say("No soy capaz de reconocer la pelota") time.sleep(5) else: print 'Se ha detectado la pelota: ', circles ball_detected = 1 # Ensure at least some circles were found if circles is not None: # Convert the (x, y) coordinates and radius of the circles to integers allowing us to draw them on our output image circles = np.round(circles[0, :]).astype("int") # Loop over the (x, y) coordinates and radius of the circles for (x, y, r) in circles: # Draw the circle in the output image, then draw a rectangle # Corresponding to the center of the circle cv2.circle(output, (x, y), r, (0, 255, 0), 4) cv2.rectangle(output, (x - 45, y - 45), (x + 45, y + 45), (0, 128, 255), -1) ### hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2HSV) # Calculate the mean of the HSV values within a rectangle (side 45) close to the center of the detected ball s = [0, 0, 0] for i in range(y - 45, y + 46): for j in range(x - 45, x + 46): s = s + hsv[i, j] #print j,i, hsv[i,j] s = s / ((45 + 46) * (45 + 46)) print s # I calculate the upper and lower limits from the mean HSV if s[1] < 100: # Saturation < 100 if (s[0] - 10) < 0: self.colourLower = (0, 40, 40) else: self.colourLower = (s[0] - 10, 40, 40) else: if (s[0] - 10) < 0: self.colourLower = (0, 100, 100) else: self.colourLower = (s[0] - 10, 100, 100) self.colourUpper = (s[0] + 10, 255, 255) print "Rango de colores. Limites superior e inferior: ", self.colourUpper, self.colourLower # Compare the colour range with the ones in the data base and say to the kid what colour is it if s[0] < 20: # Red colour = "rojo" elif s[0] > 90 and s[0] < 110: # Blue colour = "azul" elif s[0] >= 20 and s[0] <= 40: # Yellow colour = "amarillo" elif s[0] >= 130 and s[0] <= 150: # Violet colour = "violeta" else: # The colour is not in the data base colour = "no" self.ttsProxy.say("Ya la puedes coger") # Wait time.sleep(5) # 5 seconds # Go to posture stand self.postureProxy.goToPosture("StandInit", fractionMaxSpeed) if colour == "no": self.ttsProxy.say("No he visto bien el color") self.ttsProxy.say("¿Me dejas ver de nuevo la pelota?") repeate_recognition = 1 else: self.ttsProxy.say( "¿Quieres que siga una pelota de color" + colour + "¿Es eso cierto?") respuesta = 0 while (respuesta == 0): # Start the speech recognition engine with user Test_ASR self.asrProxy.subscribe("Test_ASR") self.subscribed = True print 'Speech recognition engine started' time.sleep(5) WordHeard = self.Memory.getData( "WordRecognized")[0] # Stop the speech recognition engine with user Test_ASR self.asrProxy.unsubscribe("Test_ASR") self.subscribed = False print "contador: ", count if WordHeard == "si": respuesta = 1 self.ttsProxy.say( "¡Genial! Comencemos a jugar") repeate_recognition = 0 elif WordHeard == "no": respuesta = 1 count += 1 if count > 2: self.ttsProxy.say( "¡Ai ai ai! ¿De qué color es entonces?" ) repeate_recognition = 0 respuesta2 = 0 while (respuesta2 == 0): repeate_recognition = 0 # Start the speech recognition engine with user Test_ASR self.asrProxy.subscribe("Test_ASR") self.subscribed = True print 'Speech recognition engine started' time.sleep(5) WordHeard = self.Memory.getData( "WordRecognized")[0] # Stop the speech recognition engine with user Test_ASR self.asrProxy.unsubscribe("Test_ASR") self.subscribed = False if WordHeard == "rojo": respuesta2 = 1 colour = "rojo" self.colourLower = (0, 100, 100) self.colourUpper = (20, 255, 255) self.ttsProxy.say( "Muy bien, seguiré una pelota de color" + colour) elif WordHeard == "azul": respuesta2 = 1 colour = "azul" self.colourLower = (90, 100, 100) self.colourUpper = (110, 255, 255) self.ttsProxy.say( "Muy bien, seguiré una pelota de color" + colour) elif WordHeard == "violeta": respuesta2 = 1 colour = "violeta" self.colourLower = (135, 40, 40) self.colourUpper = (155, 255, 255) self.ttsProxy.say( "Muy bien, seguiré una pelota de color" + colour) elif WordHeard == "amarillo": respuesta2 = 1 colour = "amarillo" self.colourLower = (20, 100, 100) self.colourUpper = (40, 255, 255) self.ttsProxy.say( "Muy bien, seguiré una pelota de color" + colour) else: self.ttsProxy.say( "Disculpa, tendrás que volver a repetírmelo" ) else: self.ttsProxy.say( "¡Ups! Entonces tendré que volver a ver la pelota" ) repeate_recognition = 1 else: self.ttsProxy.say( "Perdona, no te he entendido bien. ¿Puedes repetirme tu respuesta?" ) self.postureProxy.goToPosture("StandInit", fractionMaxSpeed) print "Tracker activado" self.ttsProxy.say( "Ahora muévete y enséñame la pelota, que ya voy a por ella") self.ttsProxy.say("Cuando quieras que la coja párate") self.tracking = 1 while self.end is not True: time.sleep(1) self.postureProxy.goToPosture("Stand", fractionMaxSpeed) self.ttsProxy.say("¿Quieres volver a jugar?") respuesta3 = 0 while (respuesta3 == 0): # Start the speech recognition engine with user Test_ASR self.asrProxy.subscribe("Test_ASR") self.subscribed = True print 'Speech recognition engine started' time.sleep(5) WordHeard = self.Memory.getData("WordRecognized")[0] # Stop the speech recognition engine with user Test_ASR self.asrProxy.unsubscribe("Test_ASR") self.subscribed = False if WordHeard == "si": respuesta3 = 1 self.ttsProxy.say( "¡Genial! Volvamos a jugar con una pelota de otro color") self.play_again = True elif WordHeard == "no": respuesta3 = 1 self.ttsProxy.say("Joo, ¡Qué pena!") self.play_again = False raise KeyboardInterrupt else: self.ttsProxy.say( "Perdona, no te he entendido bien. ¿Puedes repetirme tu respuesta?" )
def track_bounding_box(self, polygon): self.hunt_initiated = True self.lost_object_counter = 20 joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") #~ print polygon joint.speed = 0.1 joint.relative = True target_x = polygon.points[0].x + 0.5 * polygon.points[1].x target_y = polygon.points[0].y + 0.5 * polygon.points[1].y sub_x = target_x - 320 / 2.0 sub_y = target_y - 240 / 2.0 var_x = (sub_x / 160.0) var_y = (sub_y / 120.0) joint.joint_angles.append(-var_x * 0.05) joint.joint_angles.append(var_y * 0.05) print self.rh.sensors.getSonarsMeasurements() ans = self.rh.humanoid_motion.getJointAngles(['HeadYaw', 'HeadPitch'])['angles'] head_yaw = ans[0] head_pitch = ans[1] #~ print 'HeadPitch' + str(head_pitch) #~ print 'HeadYaw' + str(head_yaw) sonars = self.rh.sensors.getSonarsMeasurements()['sonars'] if (sonars['front_left'] <= self.sonar_value or sonars['front_right'] <= self.sonar_value) and self.lock_motion == False: self.lock_motion = True rospy.loginfo("Locked due to sonars") self.find_distance_with_sonars = True elif (head_pitch >= self.head_pitch_value or head_pitch <= -self.head_pitch_value) and self.lock_motion == False: self.lock_motion = True rospy.loginfo("Locked due to head pitch") print "self.lock_motion:", self.lock_motion if self.lock_motion is False: self.theta_vel = head_yaw * 0.1 if -self.head_yaw_value < head_yaw < self.head_yaw_value: self.x_vel = 0.3 self.pub.publish(joint) else: self.x_vel = 0 self.y_vel = 0 self.theta_vel = 0 if self.lock_motion is True: #~ self.disableObjectTracking() print "sonars:", sonars['front_left'], sonars['front_right'] print "Head_pitch:",head_pitch print "Head_yaw:",head_yaw print "Sub_x:", sub_x print "Sub_y:", sub_y dx = 0 sy = 0 if self.find_distance_with_sonars is True and\ (sonars['front_left'] <= self.sonar_value or sonars['front_right'] <= self.sonar_value): if (sonars['front_left'] <= sonars['front_right']): dx = sonars['front_left'] sy = +1 else: dx = sonars['front_right'] sy = -1 else: x = (sub_y * 47.6* 3.14159 / 180) / 240.0 print "x=", x total_x = head_pitch + x + 0.021 print "total_x=",total_x dx = 0.53 / math.tan(total_x) sy = -1 print "dx= ",dx y = (sub_x * 60.9 * 3.14159 / 180) / 320.0 print "y=", y total_y = head_yaw + sy * y print "total_y=",total_y dy = dx * math.tan(total_y) print "dy= " ,dy self.disableObjectTracking() battery = self.rh.sensors.getBatteryLevels()['levels'][0] if battery < 25: self.rh.audio.setVolume(100) self.rh.audio.speak("My battery is low") self.sub.unregister() self.rh.humanoid_motion.goToPosture("Sit", 0.7) self.rh.motion.disableMotors() sys.exit(1)
def facecallback(self, event): # Callback of the top camera of the nao_robot # Use the bridge to convert the ROS Image message to OpenCV message if self.preparado is not True: return cv_image = self.bridge.imgmsg_to_cv2(self.ros_data, desired_encoding="passthrough") face_cascade = cv2.CascadeClassifier( 'haarcascade_frontalface_default.xml') gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, scaleFactor=1.2, minNeighbors=5, minSize=(30, 30), flags=cv2.cv.CV_HAAR_SCALE_IMAGE) # Draw a rectangle around the detected faces for (x, y, w, h) in faces: cv2.rectangle(cv_image, (x, y), (x + w, y + h), (255, 0, 0), 2) # The center of the rectangle is: (x+w/2,y+h/2) # Look for the biggest face, the one to track if len(faces) > 0: c = faces[0] for i in faces: if i[2] > c[2]: c = i cv2.rectangle(cv_image, (c[0], c[1]), (c[0] + c[2], c[1] + c[3]), (255, 255, 0), 2) center = (c[0] + c[2] / 2, c[1] + c[3] / 2) # Center of the face cv2.circle(cv_image, center, 3, (0, 0, 255), -1) else: center = (cv_image.shape[1] / 2, cv_image.shape[0] / 2 ) # Image center print "No faces found" # Convert the image format from OpenCV to ROS and publish the modified image on a new topic ros_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.publisher.publish(ros_image) ### Work out the movemento of the head required to keep the ball center in the image center # Velocities message initialization joint = JointAnglesWithSpeed() joint.joint_names.append("HeadYaw") joint.joint_names.append("HeadPitch") joint.speed = 0.1 joint.relative = True #True # Get center of detected object and calculate the head turns target_x = center[0] # centroid coordenate x target_y = center[1] # centroid coordenate y # Image center -> (cv_image.shape[1]/2, cv_image.shape[0]/2)=(320/2,240/2) (x,y) coordenates sub_x = target_x - cv_image.shape[1] / 2 sub_y = target_y - cv_image.shape[0] / 2 var_x = (sub_x / float(cv_image.shape[1] / 2)) var_y = (sub_y / float(cv_image.shape[0] / 2)) # Check the Head position before moving it to restrict its movement and prevent it from crashing with the shoulders # Predict the new Y relative position posicion, the one to be restricted to avoid crashes new_position_y = self.positions[1] + var_y * 0.10 if (new_position_y <= -0.45): # Recalculate the maximun variation var_y = 0 elif (new_position_y >= 0.3): var_y = 0 joint.joint_angles.append(-var_x * 0.35) joint.joint_angles.append(var_y * 0.15) self.joint_pub.publish(joint)