def execute(self, userdata): global isToTheLeft, CURRENT_STATE CURRENT_STATE = "SeanTurn" turn_direction = 1 start_pose = [0, 0, 0, 0] if self.angle == 0: # target is goal + 0 goal = start_pose[1] elif self.angle == 90: # target is goal + turn_direction * 90 goal = start_pose[1] + np.pi / 2 * turn_direction elif self.angle == 180: # target is goal + turn_direction * 180 goal = start_pose[1] + np.pi * turn_direction elif self.angle == -90: # target is goal + turn_direction * 270 goal = start_pose[1] - np.pi / 2 * turn_direction elif self.angle == -100: # target is goal + turn_direction * 270 goal = start_pose[1] - 5 * np.pi / 9 * turn_direction elif self.angle == 120: goal = start_pose[1] + 2 * np.pi / 3 * turn_direction elif self.angle == 135: goal = start_pose[1] + 150 * np.pi / 180 * turn_direction elif self.angle == 999: if isToTheLeft: goal = start_pose[1] - 85 * np.pi / 180 * turn_direction else: goal = start_pose[1] + 85 * np.pi / 180 * turn_direction goal = angles_lib.normalize_angle(goal) cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal) speed = 0.55 rate = rospy.Rate(30) direction = turn_direction if 2 * np.pi - angles_lib.normalize_angle_positive( goal) < angles_lib.normalize_angle_positive( goal) or self.angle == 0: direction = turn_direction * -1 while not rospy.is_shutdown(): cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal) # slow down turning as we get closer to the target heading if cur < 0.1: speed = 0.1 if cur < 0.0174533: break msg = Twist() msg.linear.x = 0.0 msg.angular.z = direction * speed self.cmd_pub.publish(msg) rate.sleep() msg = Twist() msg.linear.x = 0.0 msg.angular.z = 0.0 self.cmd_pub.publish(msg) return 'done'
def execute(self, userdata): global turn_direction global START global POSE if not START: return 'exit' start_pose = POSE if self.angle == 0: # target is goal + 0 goal = start_pose[1] elif self.angle == 90: # target is goal + turn_direction * 90 goal = start_pose[1] + np.pi/2 * turn_direction elif self.angle == 180: # target is goal + turn_direction * 180 goal = start_pose[1] + np.pi * turn_direction elif self.angle == -90: # target is goal + turn_direction * 270 goal = start_pose[1] - np.pi/2 * turn_direction elif self.angle == -100: # target is goal + turn_direction * 270 goal = start_pose[1] - 5*np.pi/9 * turn_direction goal = angles_lib.normalize_angle(goal) cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal) speed = 0.55 rate = rospy.Rate(30) direction = turn_direction if 2 * np.pi - angles_lib.normalize_angle_positive(goal) < angles_lib.normalize_angle_positive(goal) or self.angle == 0: direction = turn_direction * -1 while not rospy.is_shutdown(): cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal) # slow down turning as we get closer to the target heading if cur < 0.1: speed = 0.15 if cur < 0.0571: break msg = Twist() msg.linear.x = 0.0 msg.angular.z = direction * speed self.cmd_pub.publish(msg) rate.sleep() msg = Twist() msg.linear.x = 0.0 msg.angular.z = 0.0 self.cmd_pub.publish(msg) return 'success'
def tranformation_string(res): result = [0]*len(res) for i in range(int(len(res)/3)): if 0 > res[3*i]: result[3*i] = -res[3*i] result[3*i+1] = res[3*i+1] + pi elif 0 < res[3*i]: result[3*i] = res[3*i] # a1 result[3*i+1] = res[3*i+1] # b1 # lm1 result[3*i] = normalize_angle(result[3*i]) result[3*i+1] = normalize_angle(result[3*i+1]) result[3*i+2] = res[3*i+2] * res[3*i] / sin(res[3*i] / 2) / 2 return result
def check_goal(self, x, y, angle): (trans, rot) = self.listener.lookupTransform( 'map', 'base_link', rospy.Time(0)) rpy = tf.transformations.euler_from_quaternion(rot) self.assertAlmostEqual(trans[0], x, delta=0.2) self.assertAlmostEqual(trans[1], y, delta=0.2) self.assertAlmostEqual(rpy[0], 0.0, delta=0.005) self.assertAlmostEqual(rpy[1], 0.0, delta=0.005) diff = angles.normalize_angle(rpy[2]) - angles.normalize_angle(angle) if diff > math.pi: diff = diff - math.pi if diff < -math.pi: diff = diff + math.pi self.assertAlmostEqual(diff, 0.0, delta=0.5)
def detect_from_lidar(self): time_diff = rospy.Time.now().to_sec( ) - self.enemy_position.header.stamp.to_sec() if time_diff > self.enemy_time_tolerance: # 敵情報が古かったら無視 self.detect_counter = 0 return False, 0.0, 0.0 else: self.detect_counter = self.detect_counter + 1 if self.detect_counter < self.counter_th: return False, 0.0, 0.0 map_topic = self.robot_namespace + "map" baselink_topic = self.robot_namespace + "base_link" trans, rot, vaild = self.get_position_from_tf(map_topic, baselink_topic) if vaild == False: return False, 0.0, 0.0 dx = self.enemy_position.pose.pose.position.x - trans[0] dy = self.enemy_position.pose.pose.position.y - trans[1] enemy_distance = math.sqrt(dx * dx + dy * dy) _, _, yaw = tf.transformations.euler_from_quaternion(rot) enemy_direction = math.atan2(dy, dx) enemy_direction_diff = angles.normalize_angle(enemy_direction - yaw) return True, enemy_distance, enemy_direction_diff
def execute(self, userdata): rospy.loginfo('Executing state ' + self.__class__.__name__) try: rospy.loginfo( 'Max ang vel:{}, Min ang vel:{}, Turn angle={}'.format( math.degrees(self.min_velocity), math.degrees(self.max_velocity), math.degrees(userdata.turn_angle))) pub = rospy.Publisher(self.cmd_vel, Twist, queue_size=10) odom = Odometry() odom = self.get_message() if odom is None: rospy.logerr('Cannot get oeometry') return 'ng' theta_pre = self.__get_yaw(odom) theta_sum = 0 theta_sum_max = abs(userdata.turn_angle) sign = 1 if userdata.turn_angle < 0: sign = -1 vel = Twist() vel.linear.x = 0 vel.linear.y = 0 vel.linear.z = 0 vel.angular.x = 0 vel.angular.y = 0 vel.angular.z = 0 self.time_checker.start() while self.time_checker.is_timeout() is False: odom = self.get_message() if odom is None: continue theta = self.__get_yaw(odom) d_theta = angles.normalize_angle(theta - theta_pre) theta_sum = theta_sum + abs(d_theta) theta_pre = theta finished = False if theta_sum >= theta_sum_max: vel.angular.z = 0 finished = True else: vel_abs = theta_sum_max - theta_sum vel_abs = min(self.max_velocity, vel_abs) vel_abs = max(self.min_velocity, vel_abs) vel.angular.z = sign * vel_abs rospy.loginfo('Ang vel:{}, Turn sum={}, Turn to={}'.format( math.degrees(vel.angular.z), math.degrees(theta_sum), math.degrees(theta_sum_max))) pub.publish(vel) if finished: return 'ok' return 'timeout' except Exception as e: rospy.logerr(str(e)) return 'ng'
def execute(self, userdata): global turn_direction start_pose = userdata.start_pose_in if self.angle == 0: # target is goal + 0 goal = start_pose[1] elif self.angle == 90: # target is goal + turn_direction * 90 goal = start_pose[1] + np.pi / 2 * turn_direction goal = angles_lib.normalize_angle(goal) cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal) speed = 0.55 rate = rospy.Rate(30) direction = turn_direction if self.angle == 0: direction = turn_direction * -1 while not rospy.is_shutdown(): cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal) # slow down turning as we get closer to the target heading if cur < 0.1: speed = 0.15 if cur < 0.0571: break msg = Twist() msg.linear.x = 0.0 msg.angular.z = direction * speed self.cmd_pub.publish(msg) rate.sleep() msg = Twist() msg.linear.x = 0.0 msg.angular.z = 0.0 self.cmd_pub.publish(msg) return 'success'
def pose_callback(cur_pose): lock.acquire() x = goal.x - cur_pose.x y = goal.y - cur_pose.y lock.release() dist_to_goal = math.sqrt(x**2 + y**2) error_angle = angles.shortest_angular_distance(angles.normalize_angle(cur_pose.theta), math.atan2(y, x)) velocity = Twist() if abs(dist_to_goal) > 0.05: velocity.linear.x = kf * dist_to_goal * math.cos(error_angle) velocity.angular.z = kr * error_angle vel_pub.publish(velocity)
def goToGoal(self): self.goal_d = self.goal_distance(self.goal, self.pose) if self.goal_d < 0.1: goal_direction = self.goalYaw else: goal_direction = self.goal_direction(self.goal, self.pose) self.new_heading = goal_direction if goal_direction > 0 else 2 * math.pi + goal_direction #when going to a self.goal location self.new_heading = angles.normalize_angle(self.new_heading) t_amt = angles.shortest_angular_distance(self.yaw, self.new_heading) if abs(t_amt) > self.tolerance: self.turn = True return t_amt
def pose_callback(cur_pose): lock.acquire() x = goal.x - cur_pose.x y = goal.y - cur_pose.y lock.release() dist_to_goal = math.sqrt(x**2 + y**2) error_angle = angles.shortest_angular_distance( angles.normalize_angle(cur_pose.theta), math.atan2(y, x)) velocity = Twist() if abs(dist_to_goal) > 0.05: velocity.linear.x = kf * dist_to_goal * math.cos(error_angle) velocity.angular.z = kr * error_angle vel_pub.publish(velocity)
def get_angle_and_dist_to_escape_home(detections): """Return the angle to turn and distance to drive in order to get out of the home area if the rover is trapped in there. Should be used in conjuction with Planner.is_inside_home_ring() Args: * detections - the list of AprilTagDetections Returns: * angle - the angle in radians to turn. * distance - the distance in meters to drive. Raises: * tf.Exception if the transform into the base_link frame fails. """ OVERSHOOT_DIST = 0.4 # meters, distance to overshoot target by result = {'angle': sys.maxint, 'dist': None} see_home_tag = False for detection in detections: if detection.id == 256: see_home_tag = True home_detection = swarmie.transform_pose('/base_link', detection.pose) quat = [ home_detection.pose.orientation.x, home_detection.pose.orientation.y, home_detection.pose.orientation.z, home_detection.pose.orientation.w ] _r, _p, y = tf.transformations.euler_from_quaternion(quat) y -= math.pi / 2 y = angles.normalize_angle(y) if abs(y) < result['angle']: result['angle'] = y result['dist'] = \ (math.sqrt(home_detection.pose.position.x ** 2 + home_detection.pose.position.y **2) + OVERSHOOT_DIST) if not see_home_tag: # doesn't make sense to turn or drive if no home tags were seen return 0, 0 return result['angle'], result['dist']
def update(self, encoder_counts): # left: 0, right: 1 ticks = [ encoder_counts[Constants().WHEEL_LEFT] - self._last_encs[Constants().WHEEL_LEFT], encoder_counts[Constants().WHEEL_RIGHT] - self._last_encs[Constants().WHEEL_RIGHT] ] self._last_encs = copy.deepcopy(encoder_counts) dist = [0.0, 0.0] for i in range(0, 2): dist[i] = ticks[i] / self._kinematic_parameters.ticks_per_meter total_dist = (dist[Constants().WHEEL_LEFT] + dist[Constants().WHEEL_RIGHT]) / 2.0 current_time = rospy.Time.now() d_time = (current_time - self._last_enc_time).to_sec() self._last_enc_time = current_time # TODO find better what to determine going straight, # this means slight deviation is accounted if ticks[Constants().WHEEL_LEFT] == ticks[Constants().WHEEL_RIGHT]: d_theta = 0.0 self._cur_pose.x += total_dist * cos(self._cur_pose.theta) self._cur_pose.y += total_dist * sin(self._cur_pose.theta) else: d_theta = (dist[Constants().WHEEL_RIGHT] - dist[Constants().WHEEL_LEFT]) / self.__tread new_theta = self._cur_pose.theta + d_theta radius = total_dist / d_theta self._cur_pose.x += radius * \ (sin(new_theta) - sin(self._cur_pose.theta)) self._cur_pose.y -= radius * \ (cos(new_theta) - cos(self._cur_pose.theta)) self._cur_pose.theta = angles.normalize_angle(new_theta) twist = Twist() if abs(d_time) < 0.000001: return twist else: twist.linear.x = total_dist / d_time twist.angular.z = d_theta / d_time return twist
def explore(): global turn, new_heading, turn_amt, avoid_obstacle, drd_heading, goal global ear pub = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size=1) pub_hdg_setpoint = rospy.Publisher('/hdg/setpoint', Float64, queue_size=1) pub_hdg_state = rospy.Publisher('/hdg/state', Float64, queue_size=1) sub_imu = rospy.Subscriber('/mobile_base/sensors/imu_data', Imu, callback_imu, queue_size=1) sub_bumper = rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, callback_bumper, queue_size=1) sub_hdg_pid = rospy.Subscriber('/hdg/control_effort', Float64, callback_hdg_pid, queue_size=1) sub_odom = rospy.Subscriber('/odom', Odometry, callback_odom, queue_size=1) pub_log = rospy.Publisher('/my_turtle/log', String, queue_size=1) sub_log = rospy.Subscriber('/my_turtle/log', String, callback_log, queue_size=1) rate = rospy.Rate(hz) straight = Twist() straight.linear.x = linear_vel turn_left = Twist() turn_left.angular.z = angular_vel turn_left.linear.x = 0 turn_right = Twist() turn_right.angular.z = -angular_vel turn_right.linear.x = linear_vel #-linear_vel/10.0 stop = Twist() stop.angular.z = 0 stop.linear.x = 0 x = 0 goal.x = 15 goal.y = 0 sound_intensity = 0 time.sleep(10) stops = [3, 2.7, 2.4, 2.1, 1.8, 1.5, 1.2, 0.9, 0.6, 0.2] pt = 0 stop_duration = 10 t_elapsed = 0 t = rospy.Time.now().to_sec() saved = False #check if recording has been saved. Initially no, so use False while not rospy.is_shutdown(): # and x < 10 * 60 * 4 if not ear.data is None and not ear.fft is None: aa = ear.fft #[ear.fftx>=100] # ww = ear.fftx[ear.fftx>=100] # aa = aa[ww<=500] # ww = ww[ww<=500] # print aa,ww sound_intensity = np.mean(aa) #aa[ww==500.] # sound_intensity = sound_intensity[0] sound_data.append(ear.streamdata) # sound_intensity = np.mean(aa) # print sound_intensity,aa[ww==500.] x += 1 if random.random() < turn_prob and not turn: turn = True temp_heading = yaw + random.gauss(mu, sigma) new_heading = angles.normalize_angle(temp_heading) #% 360 turn_amt = angles.shortest_angular_distance( yaw, new_heading) # (new_heading - yaw) % 360 if avoid_obstacle and not turn: if bumper.bumper == 0: #left obstacle temp_heading = yaw - math.pi / 4.0 elif bumper.bumper == 2: #right obstacle temp_heading = yaw + math.pi / 4.0 else: #front obstacle temp_heading = yaw + random.random() * math.pi + math.pi / 2.0 print bumper.state, 'bumper', bumper.bumper, temp_heading new_heading = angles.normalize_angle(temp_heading) #% 360 turn_amt = angles.shortest_angular_distance( yaw, new_heading) # (new_heading - yaw) % 360 turn = True avoid_obstacle = False drd_heading = goal_direction(goal, pose) #when going to a goal location rvel = straight if turn and False: drd_heading = new_heading t_amt = angles.shortest_angular_distance(yaw, new_heading) if turn_amt < 0: pub.publish(turn_right) # print 'tr',yaw,new_heading,turn_amt else: pub.publish(turn_left) # print 'tl',yaw,new_heading,turn_amt if abs(t_amt) < tolerance: turn = False avoid_obstacle = False else: goal_d = goal_distance(goal, pose) # rospy.loginfo("goal_d = %.2f, x = %.2f, y = %.2f, intensity = %.2f",goal_d,pose.x,pose.y,sound_intensity) # print rospy.Time.now().to_sec() standing_test = rospy.Time.now().to_sec() - t > 150 if goal_d < 0.1 or standing_test: rvel = stop ear.close() if not saved: waveFile = wave.open(sound_data_filename, 'wb') waveFile.setnchannels(1) waveFile.setsampwidth( ear.p.get_sample_size(pyaudio.paInt16)) waveFile.setframerate(ear.rate) waveFile.writeframes(b''.join(sound_data)) waveFile.close() saved = True else: rot_vel = hdg_ctrl_effort * hdg_scale if rot_vel > angular_vel: rot_vel = angular_vel if rot_vel < -angular_vel: rot_vel = -angular_vel rvel.angular.z = rot_vel if avoid_obstacle: rvel = stop if t_elapsed >= stop_duration: pt += 1 c = False if pt < len(stops) and c: print abs(goal_d - stops[pt]), t_elapsed if abs(goal_d - stops[pt]) < 0.1: rvel = stop t_elapsed += 1.0 / hz else: t_elapsed = 0 else: t_elapsed = 0 log = '{},{},{},{},{},{}'.format(rospy.Time.now().to_sec() - t, goal.x - goal_d, pose.x, pose.y, yaw, sound_intensity) print rospy.Time.now().to_sec() - t, goal_d if goal_d > 0.1 or standing_test: pub_log.publish(log) if standing_test: rvel = stop pub.publish(rvel) # if rospy.Time.now().to_sec() - t < 90: # turn_left.linear.x = 0 # turn_left.angular.z = 0.2 # pub.publish(turn_left) # else: # pub.publish(stop) # ear.close() # print 'straight',yaw,drd_heading,rot_vel set_p = drd_heading state_p = yaw if set_p < 0: set_p = set_p + math.pi * 2.0 if state_p < 0: state_p = state_p + math.pi * 2.0 # print 'straight',state_p,set_p,rot_vel pub_hdg_setpoint.publish(set_p) pub_hdg_state.publish(state_p) # print(yaw) rate.sleep() print("Quitting")
def test_normalize_angle(self, a): a = a * np.pi ref_r = normalize_angle(a) self.assertAlmostEqual(speed_up_and_execute(spw.normalize_angle, [a]), ref_r, places=5)
def add_pitch(self, val): self.mutex.acquire() self.estimated_sensor_rotation_pitch = normalize_angle( self.estimated_sensor_rotation_pitch + val) self.mutex.release()
def test_normalize_angle(self): self.assertAlmostEqual(0, normalize_angle(0)) self.assertAlmostEqual(pi, normalize_angle(pi)) self.assertAlmostEqual(0, normalize_angle(2 * pi)) self.assertAlmostEqual(pi, normalize_angle(3 * pi)) self.assertAlmostEqual(0, normalize_angle(4 * pi)) self.assertAlmostEqual(0, normalize_angle(-0)) self.assertAlmostEqual(pi, normalize_angle(-pi)) self.assertAlmostEqual(0, normalize_angle(-2 * pi)) self.assertAlmostEqual(pi, normalize_angle(-3 * pi)) self.assertAlmostEqual(0, normalize_angle(-4 * pi)) self.assertAlmostEqual(0, normalize_angle(-0)) self.assertAlmostEqual(-pi / 2, normalize_angle(-pi / 2)) self.assertAlmostEqual(pi, normalize_angle(-pi)) self.assertAlmostEqual(pi / 2, normalize_angle(-3 * pi / 2)) self.assertAlmostEqual(0, normalize_angle(-4 * pi / 2)) self.assertAlmostEqual(0, normalize_angle(0)) self.assertAlmostEqual(pi / 2, normalize_angle(pi / 2)) self.assertAlmostEqual(pi / 2, normalize_angle(5 * pi / 2)) self.assertAlmostEqual(pi / 2, normalize_angle(9 * pi / 2)) self.assertAlmostEqual(pi / 2, normalize_angle(-3 * pi / 2))
def add_roll(self, val): self.mutex.acquire() self.estimated_sensor_rotation_roll = normalize_angle( self.estimated_sensor_rotation_roll + val) self.mutex.release()
except rospy.ServiceException as e: print("Service call failed: %s" % e) def usage(): return "Usage: %s [desired x(m)] [desired y(m)] [desired yaw(deg)] \nFor example: ./walkTo.py 0.2, 0.2, 45" % \ sys.argv[0] if __name__ == "__main__": if len(sys.argv) == 4: world_x_goal = float(sys.argv[1]) world_y_goal = float(sys.argv[2]) world_yaw_goal = np.deg2rad(float(sys.argv[3])) world_yaw_goal = angles.normalize_angle(world_yaw_goal) # generate desired frame wrt world world_translation_goal = trans.translation_matrix( np.array([world_x_goal, world_y_goal, 0.0])) world_rotation_goal = trans.euler_matrix(0.0, 0.0, world_yaw_goal, 'sxyz') world_T_goal = world_translation_goal.dot(world_rotation_goal) # get walkman location wrt world # walkman_state = get_model_state('walkman') # walkman_state.pose.position.z = 0.0 # world_T_walkman = poseToMatrix(walkman_state.pose) # # world_P_walkman, world_Quaternion_walkman = poseToPositionQuaternion(walkman_state.pose) # world_RPY_walkman = trans.euler_from_quaternion(world_Quaternion_walkman)
def _calc_heading_error(self, start_xy, next_xy, robot_heading): path_delta = next_xy - start_xy path_heading = np.arctan2(path_delta[1], path_delta[0]) return normalize_angle(path_heading-robot_heading)
def explore(self,sound=True): pub = rospy.Publisher('/{}/mobile_base/commands/velocity'.format(self.robotID), Twist,queue_size=1) pub_hdg_setpoint = rospy.Publisher('/{}/hdg/setpoint'.format(self.robotID),Float64,queue_size=1) pub_hdg_state = rospy.Publisher('/{}/hdg/state'.format(self.robotID),Float64,queue_size=1) sub_imu = rospy.Subscriber('/{}/mobile_base/sensors/imu_data'.format(self.robotID),Imu,self.callback_imu,queue_size=1) sub_bumper = rospy.Subscriber('/{}/mobile_base/events/bumper'.format(self.robotID),BumperEvent,self.callback_bumper,queue_size=1) sub_hdg_pid = rospy.Subscriber('/{}/hdg/control_effort'.format(self.robotID),Float64,self.callback_hdg_pid,queue_size=1) sub_odom = rospy.Subscriber('/{}/odom'.format(self.robotID),Odometry,self.callback_odom,queue_size=1) sub_experimentStart = rospy.Subscriber('/experimentStart',Bool,self.callback_experimentStart,queue_size=10) pub_log = rospy.Publisher('/{}/log'.format(self.robotID),String,queue_size=5) # sub_log = rospy.Subscriber('/nest_move/log',String,callback_log,queue_size=1) rate = rospy.Rate(self.hz) straight = Twist() straight.linear.x = self.linear_vel turn_left = Twist() turn_left.angular.z = self.angular_vel turn_left.linear.x = 0 turn_right = Twist() turn_right.angular.z = -self.angular_vel turn_right.linear.x = self.linear_vel#-linear_vel/10.0 stop = Twist() stop.angular.z = 0 stop.linear.x = 0 x = 0 #start sound if sound is not None: #create thread for starting nest's speaker as it starts moving if sound == 'white': audio_gen = Audio_gen(filename=self.pkg_path+'White-noise-sound-20sec-mono-44100Hz.wav') else: audio_gen = Audio_gen(frequency = sound) thread = Thread(target = audio_gen.send_to_speaker) thread.start() t_elapsed = 0 goal_d = self.goal_distance(self.goal,self.pose) logheader = self.robotID + ':t,goal_d,x,y,yaw' #pause for some seconds before starting motion time.sleep(self.experimentWaitDuration) while not self.experimentStart: #busy wait till experiment start is true pub_log.publish(logheader) rate.sleep() t = rospy.Time.now().to_sec() while not rospy.is_shutdown():# and x < 10 * 60 * 4 t_elapsed = rospy.Time.now().to_sec()-t # print t_elapsed,goal_d,self.bumper.state x +=1 rvel = straight#default is straight if self.linear_vel > 0: print self.linear_vel #stop based on distance travelled goal_d = self.goal_distance(self.goal,self.pose) stopCondition = goal_d < 0.1 or self.pose.x > self.goal.x else: #stop based on experimet duration stopCondition = self.experimentDuration < t_elapsed if stopCondition: #stop robot rvel = stop #close speaker audio_gen.close_speaker() break else: rot_vel = self.hdg_ctrl_effort * self.hdg_scale * 10 if rot_vel > self.angular_vel: rot_vel = self.angular_vel if rot_vel < -self.angular_vel: rot_vel = -self.angular_vel rvel.angular.z = rot_vel #stop whenever an obstacle is hit if self.avoid_obstacle: rvel = stop # pub_log.publish(log) pub.publish(rvel) #heading control self.drd_heading = math.atan2(self.goal.y - self.pose.y, self.goal.x - self.pose.x) set_p = self.drd_heading if self.drd_heading > 0 else 2 * math.pi + self.drd_heading set_p = angles.normalize_angle(set_p) state_p = self.yaw if set_p < 0: set_p = set_p + math.pi * 2.0 if state_p < 0: state_p = state_p + math.pi * 2.0 # print 'straight',state_p,set_p,rot_vel pub_hdg_setpoint.publish(set_p) pub_hdg_state.publish(state_p) # print(yaw) log = '{}:{:.4f},{:.4f},{:.4f},{:.4f}'.format(self.robotID,goal_d,self.pose.x,self.pose.y,self.yaw) pub_log.publish(log) rospy.loginfo(str(t_elapsed) + ',' + log) rate.sleep() print("Quitting")
def explore(): global turn, new_heading, turn_amt, avoid_obstacle, drd_heading, goal global ear pub = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size=1) pub_hdg_setpoint = rospy.Publisher('/hdg/setpoint', Float64, queue_size=1) pub_hdg_state = rospy.Publisher('/hdg/state', Float64, queue_size=1) sub_imu = rospy.Subscriber('/mobile_base/sensors/imu_data', Imu, callback_imu, queue_size=1) sub_bumper = rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, callback_bumper, queue_size=1) sub_hdg_pid = rospy.Subscriber('/hdg/control_effort', Float64, callback_hdg_pid, queue_size=1) sub_odom = rospy.Subscriber('/odom', Odometry, callback_odom, queue_size=1) pub_log = rospy.Publisher('/my_turtle/log', String, queue_size=1) sub_log = rospy.Subscriber('/my_turtle/log', String, callback_log, queue_size=1) rate = rospy.Rate(hz) straight = Twist() straight.linear.x = linear_vel turn_left = Twist() turn_left.angular.z = angular_vel turn_left.linear.x = -linear_vel / 10.0 turn_right = Twist() turn_right.angular.z = -angular_vel turn_right.linear.x = -linear_vel / 10.0 stop = Twist() stop.angular.z = 0 stop.linear.x = 0 x = 0 # goal.x = 3 # goal.y = 0 sound_intensity = 0 # time.sleep(15) while not rospy.is_shutdown(): # and x < 10 * 60 * 4 if not ear.data is None and not ear.fft is None: aa = ear.fft[ear.fftx > (freq - 10)] ww = ear.fftx[(ear.fftx > (freq - 10))] aa = aa[ww < (freq + 10)] ww = ww[ww < (freq + 10)] sound_intensity = aa[0] # print sound_intensity x += 1 if random.random() < turn_prob and not turn: turn = True temp_heading = yaw + random.gauss(mu, sigma) new_heading = angles.normalize_angle(temp_heading) #% 360 turn_amt = angles.shortest_angular_distance( yaw, new_heading) # (new_heading - yaw) % 360 if avoid_obstacle and not turn: if bumper.bumper == 0: #left obstacle temp_heading = yaw - math.pi / 4.0 elif bumper.bumper == 2: #right obstacle temp_heading = yaw + math.pi / 4.0 else: #front obstacle temp_heading = yaw + random.random() * math.pi + math.pi / 2.0 print bumper.state, 'bumper', bumper.bumper, temp_heading new_heading = angles.normalize_angle(temp_heading) #% 360 turn_amt = angles.shortest_angular_distance( yaw, new_heading) # (new_heading - yaw) % 360 turn = True avoid_obstacle = False # drd_heading = goal_direction(goal,pose)#when going to a goal location if turn: drd_heading = new_heading t_amt = angles.shortest_angular_distance(yaw, new_heading) if turn_amt < 0: pub.publish(turn_right) # print 'tr',yaw,new_heading,turn_amt else: pub.publish(turn_left) # print 'tl',yaw,new_heading,turn_amt if abs(t_amt) < tolerance: turn = False avoid_obstacle = False else: # goal_d = goal_distance(goal,pose) # rospy.loginfo("goal_d = %.2f, x = %.2f, y = %.2f, intensity = %.2f",goal_d,pose.x,pose.y,sound_intensity) # print rospy.Time.now().to_sec() # if goal_d < 0.1: # straight.linear.x = 0 # straight.angular.z = 0 # else: rot_vel = hdg_ctrl_effort * hdg_scale if rot_vel > angular_vel: rot_vel = angular_vel if rot_vel < -angular_vel: rot_vel = -angular_vel straight.angular.z = rot_vel # if avoid_obstacle: # straight = stop pub.publish(straight) log = '{},{},{},{},{}'.format(rospy.Time.now().to_sec(), pose.x, pose.y, yaw, sound_intensity) pub_log.publish(log) # print 'straight',yaw,drd_heading,rot_vel set_p = drd_heading state_p = yaw if set_p < 0: set_p = set_p + math.pi * 2.0 if state_p < 0: state_p = state_p + math.pi * 2.0 # print 'straight',state_p,set_p,rot_vel pub_hdg_setpoint.publish(set_p) pub_hdg_state.publish(state_p) # print(yaw) rate.sleep() print("Quitting")
def test_normalize_angle(self): self.assertAlmostEqual(0, normalize_angle(0)) self.assertAlmostEqual(pi, normalize_angle(pi)) self.assertAlmostEqual(0, normalize_angle(2*pi)) self.assertAlmostEqual(pi, normalize_angle(3*pi)) self.assertAlmostEqual(0, normalize_angle(4*pi)) self.assertAlmostEqual(0, normalize_angle(-0)) self.assertAlmostEqual(pi, normalize_angle(-pi)) self.assertAlmostEqual(0, normalize_angle(-2*pi)) self.assertAlmostEqual(pi, normalize_angle(-3*pi)) self.assertAlmostEqual(0, normalize_angle(-4*pi)) self.assertAlmostEqual(0, normalize_angle(-0)) self.assertAlmostEqual(-pi/2, normalize_angle(-pi/2)) self.assertAlmostEqual(pi, normalize_angle(-pi)) self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2)) self.assertAlmostEqual(0, normalize_angle(-4*pi/2)) self.assertAlmostEqual(0, normalize_angle(0)) self.assertAlmostEqual(pi/2, normalize_angle(pi/2)) self.assertAlmostEqual(pi/2, normalize_angle(5*pi/2)) self.assertAlmostEqual(pi/2, normalize_angle(9*pi/2)) self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
def explore(self): pub = rospy.Publisher('/{}/mobile_base/commands/velocity'.format( self.robotID), Twist, queue_size=1) pub_hdg_setpoint = rospy.Publisher('/{}/hdg/setpoint'.format( self.robotID), Float64, queue_size=1) pub_hdg_state = rospy.Publisher('/{}/hdg/state'.format(self.robotID), Float64, queue_size=1) sub_imu = rospy.Subscriber('/{}/mobile_base/sensors/imu_data'.format( self.robotID), Imu, self.callback_imu, queue_size=1) sub_bumper = rospy.Subscriber('/{}/mobile_base/events/bumper'.format( self.robotID), BumperEvent, self.callback_bumper, queue_size=1) sub_hdg_pid = rospy.Subscriber('/{}/hdg/control_effort'.format( self.robotID), Float64, self.callback_hdg_pid, queue_size=1) sub_odom = rospy.Subscriber('/{}/robot_pose_ekf/odom_combined'.format( self.robotID), PoseWithCovarianceStamped, self.callback_odom, queue_size=1) pub_bump = rospy.Publisher('/{}/my_turtle/bump_info'.format( self.robotID), String, queue_size=1) sub_experimentStart = rospy.Subscriber('/experimentStart', Bool, self.callback_experimentStart, queue_size=1) pub_log = rospy.Publisher('/{}/log'.format(self.robotID), String, queue_size=5) # sub_log = rospy.Subscriber('/my_turtle/log',String,self.callback_log,queue_size=1) rate = rospy.Rate(self.hz) #looping rate straight = Twist() straight.angular.z = 0 straight.linear.x = self.linear_vel reverse = Twist() reverse.angular.z = 0 reverse.linear.x = -self.linear_vel turn_left = Twist() turn_left.angular.z = self.angular_vel turn_left.linear.x = 0 turn_right = Twist() turn_right.angular.z = -self.angular_vel turn_right.linear.x = 0 stop = Twist() stop.angular.z = 0 stop.linear.x = 0 x = 0 sound_intensity = 0 prev_sound = 0 curr_sound = 0 first_sound = True revStart = None # for holding location reverse motion started revD = 0.05 # reversing distance self.turn_prob = self.base_prob # initial self.turn_prob is same as self.base_prob logheader = self.robotID + ':t,x,y,yaw,prev_sound,curr_sound,turn_prob,acTion' #pause for some seconds before starting motion time.sleep(self.experimentWaitDuration) #log = logheader while not self.experimentStart: #busy wait till experiment start is true pub_log.publish(logheader) rate.sleep() # mlist = [] # mAvg = 0 # m = 0 if self.ear != None: self.ear.stream_start() t = rospy.Time.now().to_sec() while not rospy.is_shutdown(): # and x < 10 * 60 * 4 self.goal_d = np.Inf # initially set self.goal distance to be infinite to prevent stopping if self.ear != None: self.turn_prob, prev_sound, curr_sound = self.computeGrad( self.turn_prob, prev_sound, curr_sound, self.turn ) #last values of soundIntensity and self.turn_prob #chemotaxis should only be activated when sound intensity is below a threshold if curr_sound > self.theta_A: self.turn_prob = self.base_prob else: self.turn_prob = self.base_prob x += 1 bound_check = 3 if not self.turn: (bound_check, self.avoid_obstacle, bumpside, bumpyaw) = self.wall.bumper_event(self.pose, self.yaw) if self.bumper_event: self.avoid_obstacle = self.bumper_event bound_check = self.bumper self.bumper_event = False pub_bump.publish('bumper={},avoid={},{},({},{},{})'.format( bound_check, self.avoid_obstacle, bumpside, self.pose.x, self.pose.y, bumpyaw)) if self.goal != None and self.base_prob == 0: #if goal directed behaviour, go toward goal t_amt = self.goToGoal() elif rospy.Time.now().to_sec( ) - t >= self.expDuration: # if time limited experiment # Experiment duration reached. Go self.home. if self.goal == None: #if goal is not to return to a specific goal pose break else: t_amt = self.goToGoal() elif random.random( ) < self.turn_prob and not self.turn and not self.avoid_obstacle: # perform random walk self.turn = True temp_heading = self.yaw + angles.normalize_angle( random.gauss(self.mu, self.sigma)) self.new_heading = angles.normalize_angle(temp_heading) #% 360 # self.self.turn_amt = angles.shortest_angular_distance(self.yaw,self.new_heading)# (self.new_heading - self.yaw) % 360 # if self.avoid_obstacle or True: # y = self.yaw / angles.pi * 180 # if y < 0: # y = y + 360 # print bound_check,self.avoid_obstacle,self.pose.x,self.pose.y,y if bound_check != 3 and self.avoid_obstacle and not self.turn: #boundary obstacle avoidance behaviour if bound_check == 0: #left obstacle temp_heading = self.yaw - math.pi / 4.0 elif bound_check == 2: #right obstacle temp_heading = self.yaw + math.pi / 4.0 else: #front obstacle temp_heading = self.yaw + random.random( ) * math.pi + math.pi / 2.0 #print self.bumper.state,'self.bumper',self.bumper.self.bumper,temp_heading self.new_heading = angles.normalize_angle(temp_heading) #% 360 self.turn_amt = angles.shortest_angular_distance( self.yaw, self.new_heading) # (self.new_heading - self.yaw) % 360 self.turn = True self.reverseBool = True revStart = self.pose self.avoid_obstacle = False # self.drd_heading = self.goal_direction(self.goal,self.pose)#when going to a self.goal location acTion = 'straight' if self.reverseBool and revStart != None: acTion = 'reverse' #reverse for at lease revD distance before self.turning pub.publish(reverse) if self.goal_distance(revStart, self.pose) >= revD: revStart = None self.reverseBool = False elif self.turn: self.drd_heading = self.new_heading t_amt = angles.shortest_angular_distance( self.yaw, self.new_heading) if t_amt < 0: acTion = 'turnRight' pub.publish(turn_right) # print 'tr',self.yaw,self.new_heading,self.self.turn_amt else: acTion = 'turnLeft' pub.publish(turn_left) # print 'tl',self.yaw,self.new_heading,self.self.turn_amt if abs(t_amt) < self.tolerance: self.turn = False self.avoid_obstacle = False elif self.goal_d > 0.1: # self.goal_d = self.goal_distance(self.goal,self.pose) # rospy.loginfo("self.goal_d = %.2f, x = %.2f, y = %.2f, intensity = %.2f",self.goal_d,self.pose.x,self.pose.y,sound_intensity) # print rospy.Time.now().to_sec() rot_vel = self.hdg_ctrl_effort * self.hdg_scale if rot_vel > self.angular_vel: rot_vel = self.angular_vel if rot_vel < -self.angular_vel: rot_vel = -self.angular_vel straight.angular.z = rot_vel # if self.avoid_obstacle: # straight = stop pub.publish(straight) else: #t_amt = self.turnToGoalYaw() if self.goal_d <= 0.1 and abs( t_amt) < self.tolerance and rospy.Time.now().to_sec( ) - t >= self.expDuration: acTion = 'stop' #stop if self.goal (i.e. self.home in go self.home behaviour) is reached pub.publish(stop) break # print 'straight',self.yaw,self.drd_heading,rot_vel set_p = self.drd_heading state_p = self.yaw if set_p < 0: set_p = set_p + math.pi * 2.0 if state_p < 0: state_p = state_p + math.pi * 2.0 # print 'straight',state_p,set_p,rot_vel pub_hdg_setpoint.publish(set_p) pub_hdg_state.publish(state_p) # print(self.yaw) log = '{}:{:.4f},{:.4f},{:.4f},{:.4f},{:.4f},{},{}'.format( self.robotID, self.pose.x, self.pose.y, self.yaw, prev_sound, curr_sound, self.turn_prob, acTion) #,m,mAvg) pub_log.publish(log) if self.new_comm_signal: prev_sound = curr_sound # update sound intensity value self.new_comm_signal = False rospy.loginfo(log) rate.sleep() if self.ear != None: self.ear.close() print("Quitting")
def test_normalize_angle(self, a): ref_r = normalize_angle(a) self.assertAlmostEqual(w.compile_and_execute(w.normalize_angle, [a]), ref_r, places=5)
def main(self): r = rospy.Rate(self.rate) # wait for gazebo/get_model_state service to be available rospy.wait_for_service('/gazebo/get_model_state') evaluation_timeout = rospy.Time.now() + rospy.Duration.from_sec( self.race_timeout) while not rospy.is_shutdown(): current_time = rospy.Time.now() try: get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) robot_state = get_model_state(self.robotName, None) except Exception as e: print(e) # get position self.position = robot_state.pose.position # calculate orientation orientation_q = robot_state.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, self.theta) = euler_from_quaternion(orientation_list) if not self.race_started and not self.eval_finished: # check initial point if self.position.x < 0.001 and self.position.y < 0.001: self.correct_position = True elif self.correct_position: self.start_time = current_time self.race_started = True if self.debug: self.print_diagnostics('race_started', 'evaluator') elif not self.correct_position: self.disqualified = True self.disqualification_reason = 'incorrect_start' self.print_diagnostics('incorrect_start', 'evaluator') elif not self.eval_finished: # calculate sum_theta and sum_distance self.sum_theta += math.fabs( normalize_angle(self.theta - self.prev_theta)) self.sum_distance += math.sqrt(( (self.position.x - self.prev_position.x)**2) + ( (self.position.y - self.prev_position.y)**2)) # update position self.current_tile = self.update_current_tile( self.position, self.prev_position) # state checking if self.current_tile == -1: self.disqualified = True self.disqualification_reason = 'out_of_track' self.print_diagnostics('out_of_track', 'evaluator') elif self.current_tile != self.prev_tile: # we check state transition by checking if prev_tile is -1 # robot could start with tile 1, but this would be disqualified before by previous checks pt = self.tiles['track_%s' % self.prev_tile] if self.prev_tile + 1 == self.current_tile: # state transition between tiles occurs here pt.lock_tile() if self.debug: self.print_tile_stats(pt, current_time) elif self.prev_tile == (len(self.tiles) - 1) and self.current_tile == 0: # race finished pt.lock_tile() self.finished_time = current_time self.race_finished = True if self.debug: self.print_tile_stats(pt, current_time) self.print_diagnostics('race_finished', 'evaluator') else: # wrong track, disqualified self.disqualified = True self.disqualification_reason = 'out_of_sequence' self.print_diagnostics('out_of_sequence', 'evaluator') # sum total area from tracks self.get_sum_area() # notice prev_position, and prev_tile must be recorded only when race has started self.prev_position = self.position self.prev_tile = self.current_tile # notice prev theta should be recorded even if race has not started self.prev_theta = self.theta # publish evaluator data self.publish_metrics(current_time) # disqualification by timeout if (evaluation_timeout < current_time) and not self.eval_finished: self.disqualified = True self.disqualification_reason = 'out_of_time' self.print_diagnostics('out_of_time', 'evaluator') # safety self.display_metrics[ 'disqualification_reason'] = self.disqualification_reason # send result metrics self.populate_result_metrics() self.send_results(0.0, self.disqualified, **self.result_metrics) # print self metrics here, because will skip next statement because self.eval_finished will be true rospy.loginfo(self.display_metrics) self.eval_finished = True # notice race could be finished, or disqualified here if (self.race_finished or self.disqualified) and not self.eval_finished: # safety self.display_metrics[ 'disqualification_reason'] = self.disqualification_reason if self.race_finished: self.calculate_score(self.elapsed_time, self.sum_distance, self.sum_area) self.display_metrics['score'] = str(self.score) # send result metrics self.populate_result_metrics() self.send_results(self.score, self.disqualified, **self.result_metrics) rospy.loginfo(self.display_metrics) self.eval_finished = True