def getDataSendContainer(data_topic): if data_topic == 'cmd_vel': return Twist() elif data_topic == 'husky_sim_speed': model_state = ModelState() model_state.model_name = 'husky' return model_state
def animate(): pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1) rospy.init_node('animate_ball') rate = rospy.Rate(20) # 15 Hz x = 0.9 y = 0.0 z = 1.2 ballState = ModelState() ballState.model_name = 'ball' ballState.pose.orientation.x = 0 ballState.pose.orientation.y = 0 ballState.pose.orientation.z = 0 ballState.pose.orientation.w = 1 ballState.reference_frame = 'world' direction = 'left' step = 0.08 radius = 0.1 angle = 0 while not rospy.is_shutdown(): ballState.pose.position.x = x ballState.pose.position.y = y + radius * cos(angle) ballState.pose.position.z = z + radius * sin(angle) angle = angle + step % pi pub.publish(ballState) rate.sleep()
def launch_torpedo(self, srv): ''' Find position of sub and launch the torpedo from there. TODO: - Test to make sure it always fires from the right spot in the right direction. (It seems to but I haven't tested from all rotations.) ''' sub_state = self.get_model(model_name='sub8') sub_pose = msg_helpers.pose_to_numpy(sub_state.pose) # Translate torpedo init velocity so that it first out of the front of the sub. muzzle_vel = np.array([10, 0, 0]) v = geometry_helpers.rotate_vect_by_quat(np.append(muzzle_vel, 0), sub_pose[1]) launch_twist = Twist() launch_twist.linear.x = v[0] launch_twist.linear.y = v[1] launch_twist.linear.z = v[2] # This is offset so it fires approx at the torpedo launcher location. launch_pos = geometry_helpers.rotate_vect_by_quat(np.array([.4, -.15, -.3, 0]), sub_pose[1]) model_state = ModelState() model_state.model_name = 'torpedo' model_state.pose = msg_helpers.numpy_quat_pair_to_pose(sub_pose[0] + launch_pos, sub_pose[1]) model_state.twist = launch_twist self.set_torpedo(model_state) self.launched = True return True
def cbEncoder(self, encoder_msg): delta_R = (encoder_msg.x - self.previousR) * self.d_PerCount delta_L= (encoder_msg.y - self.previousL) * self.d_PerCount delta_S = (delta_R + delta_L)/2 # print "encoder_msg.x = ",encoder_msg.x," encoder_msg.y = ",encoder_msg.y #print "previousR = "******" previousL = ",self.previousL self.th = ((delta_L - delta_R)/self.Length + self.th) self.x = delta_S * math.cos(self.th) + self.x self.y = delta_S * math.sin(self.th) + self.y model_state_msg = ModelState() model_state_msg.model_name = 'duckiebot_with_gripper' model_state_msg.pose.position.x = self.x model_state_msg.pose.position.y = self.y model_state_msg.pose.position.z = 0 new_quaternion = tf.transformations.quaternion_from_euler(0, 0, self.th) model_state_msg.pose.orientation.x = new_quaternion[0] model_state_msg.pose.orientation.y = new_quaternion[1] model_state_msg.pose.orientation.z = new_quaternion[2] model_state_msg.pose.orientation.w = new_quaternion[3] self.pub_gazebo.publish(model_state_msg) self.previousR = encoder_msg.x self.previousL = encoder_msg.y
def set_model_pose(self, pose, twist=None, model='sub8'): ''' Set the position of 'model' to 'pose'. It may be helpful to kill the sub before moving it. TODO: - Deprecate kill stuff ''' set_state = yield self.nh.get_service_client('/gazebo/set_model_state', SetModelState) if twist is None: twist = numpy_to_twist([0, 0, 0], [0, 0, 0]) model_state = ModelState() model_state.model_name = model model_state.pose = pose model_state.twist = twist if model == 'sub8': # TODO: Deprecate kill stuff (Zach's PR upcoming) kill = self.nh.get_service_client('/set_kill', SetKill) yield kill(SetKillRequest(kill=Kill(id='initial', active=False))) yield kill(SetKillRequest(kill=Kill(active=True))) yield self.nh.sleep(.1) yield set_state(SetModelStateRequest(model_state)) yield self.nh.sleep(.1) yield kill(SetKillRequest(kill=Kill(active=False))) else: set_state(SetModelStateRequest(model_state))
def publish(self): gaz = ModelState() gaz.model_name = self.model_name gaz.pose = self.pose gaz.twist = self.vel self.pub.publish(gaz) rospy.loginfo(gaz) self.pose, self.vel = None, None
def racecar_reset(self): rospy.wait_for_service('gazebo/set_model_state') modelState = ModelState() modelState.pose.position.z = 0 modelState.pose.orientation.x = 0 modelState.pose.orientation.y = 0 modelState.pose.orientation.z = 0 modelState.pose.orientation.w = 0 # Use this to randomize the orientation of the car modelState.twist.linear.x = 0 modelState.twist.linear.y = 0 modelState.twist.linear.z = 0 modelState.twist.angular.x = 0 modelState.twist.angular.y = 0 modelState.twist.angular.z = 0 modelState.model_name = 'racecar' if self.world_name.startswith(MEDIUM_TRACK_WORLD): modelState.pose.position.x = -1.40 modelState.pose.position.y = 2.13 elif self.world_name.startswith(EASY_TRACK_WORLD): modelState.pose.position.x = -1.44 modelState.pose.position.y = -0.06 elif self.world_name.startswith(HARD_TRACK_WORLD): modelState.pose.position.x = 1.75 modelState.pose.position.y = 0.6 def toQuaternion(pitch, roll, yaw): cy = np.cos(yaw * 0.5) sy = np.sin(yaw * 0.5) cr = np.cos(roll * 0.5) sr = np.sin(roll * 0.5) cp = np.cos(pitch * 0.5) sp = np.sin(pitch * 0.5) w = cy * cr * cp + sy * sr * sp x = cy * sr * cp - sy * cr * sp y = cy * cr * sp + sy * sr * cp z = sy * cr * cp - cy * sr * sp return [x, y, z, w] #clockwise quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=np.pi) #anti-clockwise quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=0.0) modelState.pose.orientation.x = quaternion[0] modelState.pose.orientation.y = quaternion[1] modelState.pose.orientation.z = quaternion[2] modelState.pose.orientation.w = quaternion[3] else: raise ValueError("Unknown simulation world: {}".format(self.world_name)) self.racecar_service(modelState) time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND) self.progress_at_beginning_of_race = self.progress
def _cb_linkdata(self, msg): for pos, item in enumerate(msg.name): twist = msg.twist[pos] pose = msg.pose[pos] self.model_twists[item] = twist self.model_poses[item] = pose ms = ModelState() ms.pose = pose ms.twist = twist ms.model_name = item self.model_states[item] = ms
def reset_simulation(): global current_model_x, current_model_y, current_model_z, has_fallen # Reset the joints new_angles = {} new_angles['j_ankle1_l'] = 0.0 new_angles['j_ankle1_r'] = 0.0 new_angles['j_ankle2_l'] = 0.0 new_angles['j_ankle2_r'] = 0.0 new_angles['j_gripper_l'] = 0.0 new_angles['j_gripper_r'] = 0.0 new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body new_angles['j_low_arm_l'] = 0.0 new_angles['j_low_arm_r'] = 0.0 new_angles['j_pan'] = 0.0 new_angles['j_pelvis_l'] = 0.0 new_angles['j_pelvis_r'] = 0.0 new_angles['j_shoulder_l'] = 0.0 new_angles['j_shoulder_r'] = 0.0 new_angles['j_thigh1_l'] = 0.0 new_angles['j_thigh1_r'] = 0.0 new_angles['j_thigh2_l'] = 0.0 new_angles['j_thigh2_r'] = 0.0 new_angles['j_tibia_l'] = 0.0 new_angles['j_tibia_r'] = 0.0 new_angles['j_tilt'] = 0.0 new_angles['j_wrist_l'] = 0.0 new_angles['j_wrist_r'] = 0.0 # Default delay of setting angles is 2 seconds darwin.set_angles_slow(new_angles) # Reset the robot position # Send model to x=0,y=0 pose = Pose() pose.position.z = 0.31 twist = Twist() md_state = ModelState() md_state.model_name = model_name md_state.pose = pose md_state.twist = twist md_state.reference_frame = 'world' # Service call to reset model try: response = set_model_state(md_state) except rospy.ServiceException, e: print "Darwin model state initialization service call failed: %s" % e
def reset_simulation(self): # Reset the joints new_angles = {} new_angles['j_ankle1_l'] = 0.0 new_angles['j_ankle1_r'] = 0.0 new_angles['j_ankle2_l'] = 0.0 new_angles['j_ankle2_r'] = 0.0 new_angles['j_gripper_l'] = 0.0 new_angles['j_gripper_r'] = 0.0 new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body new_angles['j_low_arm_l'] = 0.0 new_angles['j_low_arm_r'] = 0.0 new_angles['j_pan'] = 0.0 new_angles['j_pelvis_l'] = 0.0 new_angles['j_pelvis_r'] = 0.0 new_angles['j_shoulder_l'] = 0.0 new_angles['j_shoulder_r'] = 0.0 new_angles['j_thigh1_l'] = 0.0 new_angles['j_thigh1_r'] = 0.0 new_angles['j_thigh2_l'] = 0.0 new_angles['j_thigh2_r'] = 0.0 new_angles['j_tibia_l'] = 0.0 new_angles['j_tibia_r'] = 0.0 new_angles['j_tilt'] = -0.262 # Tilt the head forward a little new_angles['j_wrist_l'] = 0.0 new_angles['j_wrist_r'] = 0.0 # Default delay of setting angles is 2 seconds self.darwin.set_angles_slow(new_angles) rospy.sleep(5) # Reset the robot position # Send model to x=0,y=0 pose = Pose() pose.position.z = 0.31 twist = Twist() md_state = ModelState() md_state.model_name = self.model_name md_state.pose = pose md_state.twist = twist md_state.reference_frame = self.relative_entity_name # Service call to reset model try: response = self.set_model_state(md_state) except rospy.ServiceException, e: self.log_write("Darwin model state initialization service call failed: " + str(e))
def turtlebot3_reset(self): rospy.wait_for_service('gazebo/set_model_state') self.x = 0 self.y = 0 # Put the turtlebot waffle at (0, 0) modelState = ModelState() modelState.pose.position.z = 0 modelState.pose.orientation.x = 0 modelState.pose.orientation.y = 0 modelState.pose.orientation.z = 0 modelState.pose.orientation.w = 0 modelState.twist.linear.x = 0 modelState.twist.linear.y = 0 modelState.twist.linear.z = 0 modelState.twist.angular.x = 0 modelState.twist.angular.y = 0 modelState.twist.angular.z = 0 modelState.model_name = 'turtlebot3' modelState.pose.position.x = self.x modelState.pose.position.y = self.y self.gazebo_model_state_service(modelState) self.burger_x = 3.5 self.burger_y = random.uniform(-1, 1) # Put the turtlebot burger at (2, 0) modelState = ModelState() modelState.pose.position.z = 0 modelState.pose.orientation.x = 0 modelState.pose.orientation.y = 0 modelState.pose.orientation.z = 0 modelState.pose.orientation.w = random.uniform(0, 3) modelState.twist.linear.x = 0 modelState.twist.linear.y = 0 modelState.twist.linear.z = 0 modelState.twist.angular.x = 0 modelState.twist.angular.y = 0 modelState.twist.angular.z = 0 modelState.model_name = 'turtlebot3_burger' modelState.pose.position.x = self.burger_x modelState.pose.position.y = self.burger_y self.gazebo_model_state_service(modelState) self.last_distance_of_turtlebot = sys.maxsize time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
def handle_position_change(self,pose): model_state = ModelState() model_state.model_name = "teresa_robot" model_state.pose = pose.pose model_state.reference_frame="map" for i in range(25): self.gazebo_pub.publish(model_state) rospy.sleep(0.03) for i in range(25): amcl_pose = PoseWithCovarianceStamped() amcl_pose.pose.pose = pose.pose amcl_pose.header.frame_id = "map" amcl_pose.header.stamp = rospy.get_rostime() self.amcl_pub.publish(amcl_pose) rospy.sleep(0.03)
def _cb_modeldata(self, msg): self.data_to_publish = {} for pos, item in enumerate(msg.name): twist = msg.twist[pos] pose = msg.pose[pos] self.model_twists[item] = twist self.model_poses[item] = pose ms = ModelState() ms.pose = pose ms.twist = twist ms.model_name = item self.model_states[item] = ms op_str = item + ", pose" val_str = str(ms.pose) self.data_to_publish[op_str] = str(val_str) if item != self.name: continue self.pose = msg.pose[pos]
def spawnMBase(x, y): mState = ModelState() mState.model_name = 'mobile_base' mState.pose.position.x = x mState.pose.position.y = y mState.pose.position.z = 0 mState.pose.orientation.x = 0 mState.pose.orientation.y = 0 mState.pose.orientation.z = 0 mState.pose.orientation.w = 1 rp.wait_for_service('/gazebo/set_model_state') try: sms = rp.ServiceProxy('/gazebo/set_model_state', SetModelState) print 'Model State: ', str(mState) sms(mState) except rp.ServiceException as e: print 'Service Exception:', e
def spawnMBase(x=DEF_X, y = DEF_Y): mState = ModelState() mState.model_name = BOT pos = Point() pos.x = x pos.y = y pos.z = 0.0 ori = Quaternion() ori.x = 0.0 ori.y = 0.0 ori.z = 0.0 ori.w = 1.0 mState.pose.position = pos mState.pose.orientation = ori mState.twist = twist_no_vel() mState.reference_frame = 'world' stop() status = serviceThis(SET_MODEL_STATE, SetModelState, mState) mbState = mState
def reset_simulation(self): # Empty the self.model_polled_z list self.model_polled_z = [] # Send model to x=0,y=0 model_name = 'darwin' pose = Pose() pose.position.z = 0.31 twist = Twist() reference_frame = 'world' md_state = ModelState() md_state.model_name = model_name md_state.pose = pose md_state.twist = twist md_state.reference_frame = reference_frame # Service call to reset model try: response = self.set_model_state(md_state) except rospy.ServiceException, e: print "Service call failed: %s"%e
def on_set_to_position_clicked_(self): success = True set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) model_state = ModelState() model_state.model_name = str(self._widget.comboBoxObjectList.currentText()).split('::')[0] model_state.pose = Pose() model_state.twist = Twist() model_state.reference_frame = "world" model_state.pose.position.x = float(str(self._widget.lineEdit_4.text())) model_state.pose.position.y = float(str(self._widget.lineEdit_5.text())) model_state.pose.position.z = float(str(self._widget.lineEdit_6.text())) try: resp1 = set_model_state(model_state) except rospy.ServiceException: success = False if success: if not resp1.success: success = False if not success: QMessageBox.warning(self._widget, "Warning", "Could not set model state.")
if msg.buttons[0]: if state < 5: state += 1 time.sleep(7) elif state == 5: state = 0 time.sleep(7) rospy.init_node('sin_path', anonymous=True) swarm_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1000) container_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10) rospy.Subscriber('/joy', Joy, joy_callback) container_msg = ModelState() container_msg.model_name = 'box' container_msg.pose.position.x = 0 container_msg.pose.position.y = 0 container_msg.pose.position.z = 0.5 swarm_msg = [ModelState() for _ in xrange(25)] for i in xrange(25): swarm_msg[i].model_name = 'quadrotor' + str(i) dt = 0.1 x = 0 y = 0 z = 0 x_values = np.arange(-5, 5, 0.0009, dtype=None)
def racecar_reset(self): rospy.wait_for_service('gazebo/set_model_state') modelState = ModelState() modelState.pose.position.z = 0 modelState.pose.orientation.x = 0 modelState.pose.orientation.y = 0 modelState.pose.orientation.z = 0 # Use this to randomize the orientation of the car modelState.pose.orientation.w = 0 modelState.twist.linear.x = 0 modelState.twist.linear.y = 0 modelState.twist.linear.z = 0 modelState.twist.angular.x = 0 modelState.twist.angular.y = 0 modelState.twist.angular.z = 0 modelState.model_name = 'racecar' if self.world_name.startswith(MEDIUM_TRACK_WORLD): modelState.pose.position.x = -1.40 modelState.pose.position.y = 2.13 elif self.world_name.startswith(EASY_TRACK_WORLD): modelState.pose.position.x = -1.44 modelState.pose.position.y = -0.06 elif self.world_name.startswith(HARD_TRACK_WORLD): wayPoints = [2, 23] wayPoint = random.choice(wayPoints) modelState.pose.position.x = self.waypoints[wayPoint][0] modelState.pose.position.y = self.waypoints[wayPoint][1] if wayPoint < 5: yaw = 0.0 else: yaw = math.pi def toQuaternion(pitch, roll, yaw): cy = np.cos(yaw * 0.5) sy = np.sin(yaw * 0.5) cr = np.cos(roll * 0.5) sr = np.sin(roll * 0.5) cp = np.cos(pitch * 0.5) sp = np.sin(pitch * 0.5) w = cy * cr * cp + sy * sr * sp x = cy * sr * cp - sy * cr * sp y = cy * cr * sp + sy * sr * cp z = sy * cr * cp - cy * sr * sp return [x, y, z, w] # clockwise quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=np.pi) # anti-clockwise quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=yaw) modelState.pose.orientation.x = quaternion[0] modelState.pose.orientation.y = quaternion[1] modelState.pose.orientation.z = quaternion[2] modelState.pose.orientation.w = quaternion[3] else: raise ValueError("Unknown simulation world: {}".format( self.world_name)) self.racecar_service(modelState) time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND) self.progress_at_beginning_of_race = self.progress
def reset(self): # Resets the state of the environment and returns an initial observation. rospy.wait_for_service('/gazebo/reset_simulation') try: self.reset_proxy() pose = Pose() pose.position.x = np.random.uniform(low=START_STATE[0] - 0.5, high=START_STATE[0] + 0.5) pose.position.y = np.random.uniform(low=START_STATE[1] - 0.5, high=START_STATE[1] + 0.5) pose.position.z = self.get_model_states( model_name="mobile_base").pose.position.z theta = np.random.uniform(low=START_STATE[2], high=START_STATE[2] + np.pi) ox, oy, oz, ow = quaternion_from_euler(0.0, 0.0, theta) pose.orientation.x = ox pose.orientation.y = oy pose.orientation.z = oz pose.orientation.w = ow reset_state = ModelState() reset_state.model_name = "mobile_base" reset_state.pose = pose self.set_model_states(reset_state) except rospy.ServiceException as e: print( "# Resets the state of the environment and returns an initial observation." ) # Unpause simulation to make observation rospy.wait_for_service('/gazebo/unpause_physics') try: self.unpause() except rospy.ServiceException as e: print("/gazebo/unpause_physics service call failed") # print("successfully unpaused!!") #laser_data = rospy.wait_for_message('/scan', LaserScan, timeout=5) # read laser data laser_data = None dynamic_data = None while laser_data is None and dynamic_data is None: try: laser_data = rospy.wait_for_message('/scan', LaserScan, timeout=5) # dynamic_data = rospy.wait_for_message('/gazebo/model_states', ModelStates, timeout=5) rospy.wait_for_service("/gazebo/get_model_state") try: dynamic_data = self.get_model_states( model_name="mobile_base") # dynamic_data = self.get_model_states(model_name="dubins_car") except rospy.ServiceException as e: print("/gazebo/unpause_physics service call failed") except: pass # print("laser data", laser_data) # print("dynamics data", dynamic_data) rospy.wait_for_service('/gazebo/pause_physics') try: self.pause() except rospy.ServiceException as e: print("/gazebo/pause_physics service call failed") obsrv = self.get_obsrv(laser_data, dynamic_data) self.pre_obsrv = obsrv # print("successfully reset!!") return np.asarray(obsrv)
def respawn(self, spawning): self.spawning = spawning if self.spawn == True or self.spawning == True: self.counter = 20000 self.counter += 1 # print self.counter else: self.counter = 0 self.spawn_publisher = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size=1) if self.vehicle_name == "fusion": xx = 55.2 yy = -100 zz = 0.3 yaw = 0 else: xx = -87.7943398547 yy = 394.797117511 zz = 0.3 yaw = -1 states = ModelState() # print self.spawning, "is the actual value" if self.counter == 0: posex = self.odom_data[0][0] posey = self.odom_data[0][1] posez = self.odom_data[0][2] poseox = self.odom_data[1][0] poseoy = self.odom_data[1][1] poseoz = self.odom_data[1][2] poseow = self.odom_data[1][3] twistx = self.linear_velocity[0] twisty = self.linear_velocity[1] twistz = self.linear_velocity[2] twistax = self.angular_velocity[0] twistay = self.angular_velocity[1] twistaz = self.angular_velocity[2] return False while self.counter > 20000: if 20000 <= self.counter < 20002: posex = self.odom_data[0][0] posey = self.odom_data[0][1] posez = self.odom_data[0][2] poseox = self.odom_data[1][0] poseoy = self.odom_data[1][1] poseoz = self.odom_data[1][2] poseow = self.odom_data[1][3] twistx = self.twist_data[0][0] twisty = self.twist_data[0][1] twistz = self.twist_data[0][2] twistax = self.twist_data[1][0] twistay = self.twist_data[1][1] twistaz = self.twist_data[1][2] self.counter += 1 time.sleep(0.2) elif 20002 <= self.counter < 20004: posex = xx posey = yy posez = zz poseox = 0 poseoy = 0 poseoz = yaw poseow = 0 twistx = 0 twisty = 0 twistz = 0 twistax = 0 twistay = 0 twistaz = 0 self.counter += 1 time.sleep(0.2) if self.counter >= 20004: # print self.counter self.counter = 0 # print self.counter states.model_name = "%s" % (self.vehicle_name) states.twist.linear.x = twistx states.twist.linear.y = twisty states.twist.linear.z = twistz states.twist.angular.x = twistax states.twist.angular.y = twistay states.twist.angular.z = twistaz states.pose.position.x = posex states.pose.position.y = posey states.pose.position.z = posez states.pose.orientation.x = poseox states.pose.orientation.y = poseoy states.pose.orientation.z = poseoz states.pose.orientation.w = poseow self.spawn_publisher.publish(states) return False
def __init__(self, robot_name, max_steps): #------------Params-------------------- self.depth_image_size = [160, 128] self.rgb_image_size = [304, 228] self.bridge = CvBridge() self.robot_name = robot_name self.self_speed = [0.0, 0.0] self.start_time = time.time() self.max_steps = max_steps self.sim_time = Clock().clock self.control_freq = 5. self.pose = None default_positions = { 'robot1': [11., 0., 0.], 'robot2': [11., 1., 0.], 'robot3': [11., 2., 0.], 'robot4': [11., 3., 0.], 'robot5': [11., 4., 0.] } #-----------Default Robot State----------------------- self.default_state = ModelState() self.default_state.model_name = robot_name self.default_state.pose.position.x = default_positions[robot_name][0] self.default_state.pose.position.y = default_positions[robot_name][1] self.default_state.pose.position.z = default_positions[robot_name][2] self.default_state.pose.orientation.x = 0.0 self.default_state.pose.orientation.y = 0.0 self.default_state.pose.orientation.z = 0.0 self.default_state.pose.orientation.w = 1.0 self.default_state.twist.linear.x = 0. self.default_state.twist.linear.y = 0. self.default_state.twist.linear.z = 0. self.default_state.twist.angular.x = 0. self.default_state.twist.angular.y = 0. self.default_state.twist.angular.z = 0. self.default_state.reference_frame = 'world' #-----------Publisher and Subscriber------------- self.cmd_vel = rospy.Publisher(robot_name + '/mobile_base/commands/velocity', Twist, queue_size=10) self.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=100) self.resized_depth_img = rospy.Publisher(robot_name + '/camera/depth/image_resized', Image, queue_size=10) self.resized_rgb_img = rospy.Publisher(robot_name + '/camera/rgb/image_resized', Image, queue_size=10) self.object_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack) self.odom_sub = rospy.Subscriber(robot_name + '/odom', Odometry, self.OdometryCallBack) self.sim_clock = rospy.Subscriber('clock', Clock, self.SimClockCallBack) self.depth_image_sub = rospy.Subscriber( robot_name + '/camera/depth/image_raw', Image, self.DepthImageCallBack) self.rgb_image_sub = rospy.Subscriber( robot_name + '/camera/rgb/image_raw', Image, self.RGBImageCallBack) self.laser_sub = rospy.Subscriber('mybot/laser/scan', LaserScan, self.LaserScanCallBack)
is_first_while = True pre_test_number = -1 rospy.loginfo(log_file_path) while not rospy.is_shutdown(): if test_number != pre_test_number: end_time = rospy.Time.now() if not is_first_while: elapsed_time = (end_time - start_time).to_sec() rospy.loginfo("{}".format(elapsed_time)) f = open(log_file_path, 'a') f.write("{}, {}\n".format(path_list[test_number], elapsed_time)) f.close() is_first_while = False rospy.wait_for_service('gazebo/set_model_state') init_pose = Pose(position=Point(-1,0,1.5), orientation=Quaternion(0, 0, 0.7071068, 0.7071068)) model_init_state = ModelState(model_name='ackermann_vehicle', pose=init_pose, twist=Twist(), reference_frame="world") spawn_model_prox = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) spawn_model_prox(model_init_state) path_reader(path_list[test_number]) rospy.loginfo("{} {}".format(test_number, path_list[test_number])) pre_test_number = test_number is_first_while = False wp_index = 1 vehicle_status = "Generated" start_time = end_time try: rospy.spin() except rospy.ROSInterruptException: pass
def pub_odometry(self): """ Publish odometry message """ odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.frame_id odom.child_frame_id = self.world_frame_id odom.pose.pose.position.x = self.p[0] odom.pose.pose.position.y = self.p[1] odom.pose.pose.position.z = self.p[2] orientation = tf.transformations.quaternion_from_euler( self.p[3], self.p[4], self.p[5], 'sxyz') odom.pose.pose.orientation.x = orientation[0] odom.pose.pose.orientation.y = orientation[1] odom.pose.pose.orientation.z = orientation[2] odom.pose.pose.orientation.w = orientation[3] odom.twist.twist.linear.x = self.v[0] odom.twist.twist.linear.y = self.v[1] odom.twist.twist.linear.z = self.v[2] odom.twist.twist.angular.x = self.v[3] odom.twist.twist.angular.y = self.v[4] odom.twist.twist.angular.z = self.v[5] self.pub_odom.publish(odom) # Broadcast transform br = tf.TransformBroadcaster() br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation, odom.header.stamp, odom.header.frame_id, self.world_frame_id) ################################################################ ########### Publish position for gazebo ################ ################################################################ gazebo_odom = ModelState() ###### TODO: WARNING! Gazebo uses Z up configuraton!!! ### # I've created a custom rotation that rotates the position 180 degress # in roll, but the orientation transformation is only yaw = -yaw. # CHECK WHAT HAPPENS WHITH ROLL AND PITCH! """rot = tf.transformations.euler_matrix(math.pi, 0.0, 0.0) position = np.matrix([odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z, 1.0]).reshape(4, 1) new_position = rot * position eulr = tf.transformations.euler_from_quaternion([odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w]) new_quat = tf.transformations.quaternion_from_euler(eulr[0], eulr[1], -eulr[2]) gazebo_odom.model_name = 'girona500' gazebo_odom.pose.position.x = float(new_position[0]) gazebo_odom.pose.position.y = float(new_position[1]) gazebo_odom.pose.position.z = float(new_position[2]) + self.sea_bottom_depth gazebo_odom.pose.orientation = odom.pose.pose.orientation gazebo_odom.pose.orientation.x = new_quat[0] gazebo_odom.pose.orientation.y = new_quat[1] gazebo_odom.pose.orientation.z = new_quat[2] gazebo_odom.pose.orientation.w = new_quat[3] gazebo_odom.reference_frame = 'world' self.pub_odom_gazebo.publish(gazebo_odom) # Brodcast world to girona50000_gazebo_link br = tf.TransformBroadcaster() br.sendTransform((gazebo_odom.pose.position.x, gazebo_odom.pose.position.y, gazebo_odom.pose.position.z), new_quat, odom.header.stamp, "girona500_gazebo_link", self.world_frame_id) """ ################################################################## # ALTERNATIVE WITH NO ROTATIONS ##### gazebo_odom.model_name = 'girona500' gazebo_odom.pose = odom.pose.pose gazebo_odom.reference_frame = 'world' self.pub_odom_gazebo.publish(gazebo_odom)
def __init__(self): # initiliaze rospy.init_node('GazeboWorld', anonymous=False) #-----------Default Robot State----------------------- self.set_self_state = ModelState() self.set_self_state.model_name = 'mobile_base' self.set_self_state.pose.position.x = 0. self.set_self_state.pose.position.y = 2.5 self.set_self_state.pose.position.z = 0. self.set_self_state.pose.orientation.x = 0.0 self.set_self_state.pose.orientation.y = 0.0 self.set_self_state.pose.orientation.z = 0.0 self.set_self_state.pose.orientation.w = 1.0 self.set_self_state.twist.linear.x = 0. self.set_self_state.twist.linear.y = 0. self.set_self_state.twist.linear.z = 0. self.set_self_state.twist.angular.x = 0. self.set_self_state.twist.angular.y = 0. self.set_self_state.twist.angular.z = 0. self.set_self_state.reference_frame = 'world' #------------Params-------------------- self.depth_image_size = [160, 128] self.rgb_image_size = [304, 228] self.bridge = CvBridge() self.object_state = [0, 0, 0, 0] self.object_name = [] # 0. | left 90/s | left 45/s | right 45/s | right 90/s | acc 1/s | slow down -1/s self.action_table = [ [0.5, np.pi / 12], [0.5, 0], [0.5, -np.pi / 12], [0.2, np.pi / 6], [0.2, -np.pi / 6], [0.2, np.pi / 12], [0.2, -np.pi / 12], [0.2, 0], [0, np.pi / 6], [0, -np.pi / 6] ] #[0.4, 0.2, np.pi/6, np.pi/12, 0., -np.pi/12, -np.pi/6] self.self_speed = [.4, 0.0] self.default_states = None self.start_time = time.time() self.max_steps = 10000 self.beam_num = 512 self.depth_image = None self.bump = False #-----------Publisher and Subscriber------------- self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) self.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10) self.resized_depth_img = rospy.Publisher('camera/depth/image_resized', Image, queue_size=10) self.resized_rgb_img = rospy.Publisher('camera/rgb/image_resized', Image, queue_size=10) self.object_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack) self.depth_image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.RGBImageCallBack) self.rgb_image_sub = rospy.Subscriber('camera/depth/image_raw', Image, self.DepthImageCallBack) self.laser_sub = rospy.Subscriber('scan', LaserScan, self.LaserScanCallBack) self.odom_sub = rospy.Subscriber('odom', Odometry, self.OdometryCallBack) self.bumper_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.BumperCallBack) rospy.sleep(2.) # What function to call when you ctrl + c rospy.on_shutdown(self.shutdown)
def main(): global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_ global srv_client_go_to_point_, srv_client_wall_follower_ rospy.init_node('bug0') sub_laser = rospy.Subscriber('/diff_bot/laser/scan', LaserScan, clbk_laser) sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom) rospy.wait_for_service('/go_to_point_switch') rospy.wait_for_service('/wall_follower_switch') rospy.wait_for_service('/gazebo/set_model_state') srv_client_go_to_point_ = rospy.ServiceProxy('/go_to_point_switch', SetBool) srv_client_wall_follower_ = rospy.ServiceProxy('/wall_follower_switch', SetBool) srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # set robot position model_state = ModelState() model_state.model_name = 'diff_bot' model_state.pose.position.x = initial_position_.x model_state.pose.position.y = initial_position_.y resp = srv_client_set_model_state(model_state) # initialize going to the point change_state(0) rate = rospy.Rate(20) while not rospy.is_shutdown(): if regions_ == None: continue if state_ == 0: if regions_['front'] > 0.15 and regions_['front'] < 1: change_state(1) elif state_ == 1: desired_yaw = math.atan2(desired_position_.y - position_.y, desired_position_.x - position_.x) err_yaw = normalize_angle(desired_yaw - yaw_) # less than 30 degrees if math.fabs(err_yaw) < (math.pi / 6) and \ regions_['front'] > 1.5 and regions_['fright'] > 1 and regions_['fleft'] > 1: print 'less than 30' change_state(0) # between 30 and 90 if err_yaw > 0 and \ math.fabs(err_yaw) > (math.pi / 6) and \ math.fabs(err_yaw) < (math.pi / 2) and \ regions_['left'] > 1.5 and regions_['fleft'] > 1: print 'between 30 and 90 - to the left' change_state(0) if err_yaw < 0 and \ math.fabs(err_yaw) > (math.pi / 6) and \ math.fabs(err_yaw) < (math.pi / 2) and \ regions_['right'] > 1.5 and regions_['fright'] > 1: print 'between 30 and 90 - to the right' change_state(0) rate.sleep()
rospy.logerr('Error on calling service: %s',str(e)) pose = Pose() pose.position.x = rospy.get_param('~position_x') pose.position.y = rospy.get_param('~position_y') pose.position.z = rospy.get_param('~position_z') pose.orientation.x = rospy.get_param('~orientation_x') pose.orientation.y = rospy.get_param('~orientation_y') pose.orientation.z = rospy.get_param('~orientation_z') pose.orientation.w = rospy.get_param('~orientation_w') state = ModelState() state.model_name = "robot" state.pose = pose rospy.loginfo("Moving robot") try: ret = g_set_state(state) print ret.status_message except Exception, e: rospy.logerr('Error on calling service: %s',str(e))
#-*-coding: utf8-*- """ Remplace le systeme VICON pour la simulation en recuperant la position des objets dans gazebo et en les publiant. Met également à jour les positions des objectifs des drones dans gazebo (invisible en realite). """ import rospy from tf.transformations import euler_from_quaternion, quaternion_from_euler from gazebo_msgs.msg import ModelState, ModelStates from geometry_msgs.msg import PoseStamped, TransformStamped pos_leader = [0, 0, 0, 0] pos_follower = [0, 0, 0, 0] ms_leader = ModelState() ms_follower = ModelState() def recupere_pos(msg): global pos_leader, pos_follower #leader i = msg.name.index("uav1") pos_leader[0] = msg.pose[i].position.x pos_leader[1] = msg.pose[i].position.y pos_leader[2] = msg.pose[i].position.z q = (msg.pose[i].orientation.x, msg.pose[i].orientation.y, msg.pose[i].orientation.z, msg.pose[i].orientation.w) euler = euler_from_quaternion(q) pos_leader[3] = euler[2]
if abs(X) > math.pi / 2 or abs(Y) > math.pi / 2: return True else: return False def reset(self): rospy.wait_for_service('/gazebo/reset_world') try: reset_world_handle = rospy.ServiceProxy('/gazebo/reset_world', Empty) reset_world_handle() except rospy.ServiceException, e: print "Service call failed: %s" % e init_model_state = ModelState() if self.random_seed != None: if self.random_seed != 0: random.seed(self.random_seed) init_model_state.model_name = 'iris' init_model_state.pose.position.x = random.uniform(-3, 3) init_model_state.pose.position.y = random.uniform(-3, 3) init_model_state.pose.position.z = random.uniform(2, 8) w, x, y, z = self.randQ() init_model_state.pose.orientation.x = x init_model_state.pose.orientation.y = y init_model_state.pose.orientation.z = z init_model_state.pose.orientation.w = w init_model_state.twist.linear.x = random.uniform(-1, 1) init_model_state.twist.linear.y = random.uniform(-1, 1) init_model_state.twist.linear.z = random.uniform(-1, 1)
def reset(self, episode_num, collision): print "Resetting Baxter..." # self.delete_model("hugging_target") # rospy.sleep(0.1) model_msg = ModelState() model_msg.model_name = "humanoid" model_msg.reference_frame = "world" model_msg.pose.position.x = 2.0 model_msg.pose.position.y = 0.0 model_msg.pose.position.z = 0 # self.set_model_config('hugging_target', 'humanoids', # ['LeftLeg_joint', 'RightLeg_joint', 'RightUpperArm_joint', 'Head_joint'], # [0.0, 0.0, 0.0, 0.0]) resp_set = self.set_model_state(model_msg) # reset arm position if not self.use_moveit: # right arm start_point_right = [-0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0] t01 = threading.Thread(target=resetarm_job, args=(self.right_limb_interface, start_point_right)) t01.start() # left arm start_point_left = [0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0] t02 = threading.Thread(target=resetarm_job, args=(self.left_limb_interface, start_point_left)) t02.start() t02.join() t01.join() else: # Moveit joint move joint_goal = self.group.get_current_joint_values() # s0, s1, e0, e1, w0, w1, w2 joint_goal[0] = 0.0 joint_goal[1] = -0.55 joint_goal[2] = 0.0 joint_goal[3] = 0.75 joint_goal[4] = 0.0 joint_goal[5] = 1.26 joint_goal[6] = 0.0 self.group.go(joint_goal, wait=True) self.group.stop() # ###################### Reset hugging target ########################## if self.hug_target == 1: # Randomly initialize target position self.cylinder1[0] = random.uniform(0.3, 0.8) self.cylinder1[1] = random.uniform(-0.2, 0.2) cylinder_z = self.cylinder_height / 2.0 for i in range(len(self.target_line_start)): self.target_line_start[i] = self.cylinder1 + asarray( [0, 0, self.cylinder_height]) / 9.0 * i self.target_line = self.target_line_start print "load gazebo model" resp = self.load_model( "hugging_target", "cylinder.sdf", Pose(position=Point( x=self.cylinder1[0], y=self.cylinder1[1], z=cylinder_z))) if self.hug_target == 2: self.target_line_start = self.target_line_start - self.target_pos_start self.target_pos_start[0] = 0.6 #random.uniform(0.4, 0.8) self.target_pos_start[1] = 0.0 #random.uniform(-0.2, 0.2) self.target_line_start = self.target_line_start + self.target_pos_start self.target_line = self.target_line_start print "load gazebo model" # resp = self.load_model("hugging_target", "humanoid/humanoid.urdf", # Pose(position=Point(x=self.target_pos_start[0], y=self.target_pos_start[1], z=0)), type="urdf") # rospy.sleep(0.1) model_msg = ModelState() model_msg.model_name = "humanoid" model_msg.reference_frame = "world" model_msg.pose.position.x = self.target_pos_start[0] model_msg.pose.position.y = self.target_pos_start[1] model_msg.pose.position.z = -0.5 resp_set = self.set_model_state(model_msg) rospy.sleep(0.5) # Listen to collision information # rospy.Subscriber(self.collision_topic, String, self.collision_getter) if self.use_moveit: # Delete object in the scene count = 0 while 'target' in self.scene.getKnownCollisionObjects(): count += 1 if count > 10: self.scene._collision = [] self.scene.removeCollisionObject('target', wait=True) rospy.sleep(0.1) print "deleting target...", cylinder_name = "target" cylinder_height = self.cylinder_height # Add object to the planning scene for collision avoidance while 'target' not in self.scene.getKnownCollisionObjects(): self.scene.addCylinder( cylinder_name, cylinder_height, self.cylinder_radius, self.cylinder1[0], self.cylinder1[1], self.cylinder1[2] + cylinder_height / 2.0) rospy.sleep(0.1) print "adding target..." print "cylinder_x: %f, cylinder_y: %f" % (self.cylinder1[0], self.cylinder1[1]), print "done"
def reset(self): ''' Resettng the Experiment 1. Update the counter based on the flag from step 2. Assign next positions and reset the positions of robots and targets ''' # ========================================================================= # # 1. TARGET UPDATE # # ========================================================================= # self.is_collided = False angle_target = self.target_index * 2 * pi / self.num_experiments self.pos_x = self.radius * cos(angle_target) self.pos_y = self.radius * sin(angle_target) if self.target_index < self.num_experiments - 1: self.target_index += 1 else: self.target_index = 0 self.theta = angle_target quat = quaternion_from_euler(0, 0, self.theta + pi) # ========================================================================= # # 2. RESET # # ========================================================================= # # Reset Turtlebot position state_msg = ModelState() state_msg.model_name = 'turtlebot3_waffle_pi' state_msg.pose.position.x = -0.5 state_msg.pose.position.y = 0.0 state_msg.pose.position.z = 0.0 state_msg.pose.orientation.x = 0 state_msg.pose.orientation.y = 0 state_msg.pose.orientation.z = 0 state_msg.pose.orientation.w = 0 # state_msg.pose.position.x = self.pos_x # state_msg.pose.position.y = self.pos_y # state_msg.pose.position.z = 0.0 # state_msg.pose.orientation.x = quat[0] # state_msg.pose.orientation.y = quat[1] # state_msg.pose.orientation.z = quat[2] # state_msg.pose.orientation.w = quat[3] # Reset Target Position state_target_msg = ModelState() state_target_msg.model_name = 'unit_sphere' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0' state_target_msg.pose.position.x = self.target_x state_target_msg.pose.position.y = self.target_y state_target_msg.pose.position.z = 0.0 state_target_msg.pose.orientation.x = 0 state_target_msg.pose.orientation.y = 0 state_target_msg.pose.orientation.z = -0.2 state_target_msg.pose.orientation.w = 0 rospy.wait_for_service('gazebo/reset_simulation') rospy.wait_for_service('/gazebo/set_model_state') try: set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(state_msg) resp_targ = set_state(state_target_msg) except rospy.ServiceException as e: print("Service Call Failed: %s" % e) initial_state = np.zeros(self.state_num) self.move_cmd.linear.x = 0.0 self.move_cmd.angular.z = 0.0 self.pub.publish(self.move_cmd) time.sleep(1) self.pub.publish(self.move_cmd) self.rate.sleep() rospy.wait_for_service('phero_reset') try: phero_reset = rospy.ServiceProxy('phero_reset', PheroReset) resp = phero_reset(True) print("Reset Pheromone grid successfully: {}".format(resp)) except rospy.ServiceException as e: print("Service Failed %s" % e) return range(0, self.num_robots), initial_state
def _reset(self): # Unpause simulation to make observation rospy.wait_for_service('/gazebo/unpause_physics') try: self.unpause() except (rospy.ServiceException) as e: print("/gazebo/unpause_physics service call failed") try: # Wait for robot to be ready to accept signal rospy.wait_for_message('motor_state', MotorSignal, timeout=5) motor_signal = MotorSignal() motor_signal.motor_type = 1 motor_signal.signal = [512, 512, 512, 512, 512, 512, 512, 512] self.action_publisher.publish(motor_signal) except: print("cannot publish action") time.sleep(.5) # return to initial joint positions rospy.wait_for_service('/gazebo/set_model_configuration') try: joint_names = [ 'base_to_femur_1', 'base_to_femur_2', 'base_to_femur_3', 'base_to_femur_4', 'femur_to_tibia_1', 'femur_to_tibia_2', 'femur_to_tibia_3', 'femur_to_tibia_4' ] joint_positions = [0, 0, 0, 0, 0, 0, 0, 0] self.set_model_config_proxy('spyndra', 'spyndra_description', joint_names, joint_positions) except (rospy.ServiceException) as e: print("/gazebo/set_model_configuration service call failed") time.sleep(.5) # return to initial model position rospy.wait_for_service('/gazebo/set_model_state') try: model_state = ModelState() model_state.model_name = 'spyndra' model_state.pose.position.x, model_state.pose.position.y, model_state.pose.position.z = 0, 0, 0.5 model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w = 0, 0, 0, 0 model_state.twist.linear.x, model_state.twist.linear.y, model_state.twist.linear.z = 0, 0, 0 model_state.twist.angular.x, model_state.twist.angular.y, model_state.twist.angular.z = 0, 0, 0 self.set_model_state_proxy(model_state) except (rospy.ServiceException) as e: print("/gazebo/set_model_state service call failed") time.sleep(1) # stand up try: # Wait for robot to be ready to accept signal rospy.wait_for_message('motor_state', MotorSignal, timeout=5) motor_signal = MotorSignal() motor_signal.motor_type = 1 motor_signal.signal = [512, 512, 512, 512, 820, 820, 820, 820] self.action_publisher.publish(motor_signal) except: print("cannot publish action") time.sleep(7) # initiate observation s_ = np.zeros(self.nS) # older gazebo version always have different initial IMU info after reset, keep them zero # # imu data update # imu_data = None # while imu_data is None: # try: # imu_data = rospy.wait_for_message('imu', Imu) # s_[8: 14] = [imu_data.linear_acceleration.x, imu_data.linear_acceleration.y, imu_data.linear_acceleration.z, \ # imu_data.angular_velocity.x, imu_data.angular_velocity.y, imu_data.angular_velocity.z] # except: # pass # motor data update motor_data = None while motor_data is None: try: motor_data = rospy.wait_for_message('motor_state', MotorSignal) s_[:8] = motor_data.signal except: pass # position data update position_data = None while position_data is None: try: position_data = rospy.wait_for_message('/gazebo/model_states', ModelStates, timeout=5) index = position_data.name.index("spyndra") #s_[8: 11] = x, y, z = [ position_data.pose[index].position.x, position_data.pose[index].position.y, position_data.pose[index].position.y ] self.INIT_POS = np.array([x, y, z]) self.GOAL = np.array(self.INIT_POS, copy=True) self.GOAL[0] += 10 self.INIT_DIST = np.linalg.norm(self.INIT_POS - self.GOAL) except: pass rospy.wait_for_service('/gazebo/pause_physics') try: #resp_pause = pause.call() self.pause() except (rospy.ServiceException) as e: print("/gazebo/pause_physics service call failed") self.START_TIME = time.time() return s_
def main(): global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_ global srv_client_go_to_point_, srv_client_wall_follower_ rospy.init_node('explorer1') sub_laser = rospy.Subscriber('/uav1/scan', LaserScan, clbk_laser) sub_odom = rospy.Subscriber('/uav1/ground_truth_to_tf', Odometry, clbk_odom) #pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) rospy.wait_for_service('/go_to_point_switch1') rospy.wait_for_service('/wall_follower_switch1') rospy.wait_for_service('/gazebo/set_model_state') srv_client_go_to_point_ = rospy.ServiceProxy('/go_to_point_switch1', SetBool) srv_client_wall_follower_ = rospy.ServiceProxy('/wall_follower_switch1', SetBool) srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # set robot position model_state = ModelState() model_state.model_name = 'uav1' model_state.pose.position.x = initial_position_.x model_state.pose.position.y = initial_position_.y resp = srv_client_set_model_state(model_state) # initialize going to the point change_state(0) rate = rospy.Rate(20) while not rospy.is_shutdown(): if regions_ == None: continue if state_ == 0: if regions_['front'] < 1.5 and regions_['fright'] <1.5 and regions_['fleft'] < 1.5 and regions_['right'] < 1.5 and regions_['left'] < 1.5: change_state(1) elif state_ == 1: desired_yaw = math.atan2(desired_position_.y - position_.y, desired_position_.x - position_.x) err_yaw = normalize_angle(desired_yaw - yaw_) # less than 30 degrees #if math.fabs(err_yaw) < (math.pi / 6) and \ #regions_['front'] > 1.5 and regions_['fright'] > 1.5 and regions_['fleft'] > 1.5: #print 'less than 30' #change_state(0) # between 30 and 90 #if err_yaw > 0 and \ #math.fabs(err_yaw) > (math.pi / 6) and \ #math.fabs(err_yaw) < (math.pi / 2) and \ #regions_['left'] > 1.5 and regions_['fleft'] > 1: #print 'between 30 and 90 - to the left' #change_state(0) #if err_yaw < 0 and \ #math.fabs(err_yaw) > (math.pi / 6) and \ #math.fabs(err_yaw) < (math.pi / 2) and \ #regions_['right'] > 1.5 and regions_['fright'] > 1: #print 'between 30 and 90 - to the right' #change_state(0) if regions_['front'] > 2 and regions_['fright'] > 2 and regions_['fleft'] > 2 and regions_['right'] > .4 and regions_['left'] > .4: change_state(0) rate.sleep()
def moving(self): state = 0 while not rospy.is_shutdown(): model = rospy.wait_for_message('gazebo/model_states', ModelStates) for i in range(len(model.name)): if model.name[i] == 'obstacle_1': obstacle_1 = ModelState() obstacle_1.model_name = model.name[i] obstacle_1.pose = model.pose[i] if abs(obstacle_1.pose.position.x - 2) < 0.05 and abs( obstacle_1.pose.position.y - 2) < 0.05: state = 0 if state == 0: obstacle_1.pose.position.x -= 0.005 obstacle_1.pose.position.y -= 0.012 if abs(obstacle_1.pose.position.x - 1.5) < 0.05 and abs(obstacle_1.pose.position.y - 0.8) < 0.05: state = 1 elif state == 1: obstacle_1.pose.position.x -= 0.01 obstacle_1.pose.position.y += 0.007 if abs(obstacle_1.pose.position.x - 0.5) < 0.05 and abs(obstacle_1.pose.position.y - 1.5) < 0.05: state = 2 elif state == 2: obstacle_1.pose.position.x -= 0.008 obstacle_1.pose.position.y -= 0.002 if abs(obstacle_1.pose.position.x + 0.3) < 0.05 and abs(obstacle_1.pose.position.y - 1.3) < 0.05: state = 3 elif state == 3: obstacle_1.pose.position.x -= 0.007 obstacle_1.pose.position.y -= 0.005 if abs(obstacle_1.pose.position.x + 1) < 0.05 and abs( obstacle_1.pose.position.y - 0.8) < 0.05: state = 4 elif state == 4: obstacle_1.pose.position.x -= 0.01 obstacle_1.pose.position.y += 0.007 if abs(obstacle_1.pose.position.x + 2) < 0.05 and abs( obstacle_1.pose.position.y - 1.5) < 0.05: state = 5 elif state == 5: obstacle_1.pose.position.x -= 0.002 obstacle_1.pose.position.y -= 0.007 if abs(obstacle_1.pose.position.x + 2.2) < 0.05 and abs(obstacle_1.pose.position.y - 0.8) < 0.05: state = 6 elif state == 6: obstacle_1.pose.position.x += 0.004 obstacle_1.pose.position.y -= 0.011 if abs(obstacle_1.pose.position.x + 1.8) < 0.05 and abs(obstacle_1.pose.position.y + 0.3) < 0.05: state = 7 elif state == 7: obstacle_1.pose.position.x += 0.003 obstacle_1.pose.position.y -= 0.007 if abs(obstacle_1.pose.position.x + 1.5) < 0.05 and abs(obstacle_1.pose.position.y + 1) < 0.05: state = 8 elif state == 8: obstacle_1.pose.position.x += 0.009 obstacle_1.pose.position.y += 0.007 if abs(obstacle_1.pose.position.x + 0.6) < 0.05 and abs(obstacle_1.pose.position.y + 0.3) < 0.05: state = 9 elif state == 9: obstacle_1.pose.position.x += 0.011 obstacle_1.pose.position.y -= 0.011 if abs(obstacle_1.pose.position.x - 0.5) < 0.05 and abs(obstacle_1.pose.position.y + 1.4) < 0.05: state = 10 elif state == 10: obstacle_1.pose.position.x += 0.006 obstacle_1.pose.position.y -= 0.006 if abs(obstacle_1.pose.position.x - 1.1) < 0.05 and abs(obstacle_1.pose.position.y + 2) < 0.05: state = 11 elif state == 11: obstacle_1.pose.position.x += 0.009 obstacle_1.pose.position.y += 0.01 if abs(obstacle_1.pose.position.x - 2) < 0.05 and abs( obstacle_1.pose.position.y + 1) < 0.05: state = 12 elif state == 12: obstacle_1.pose.position.x -= 0.008 obstacle_1.pose.position.y += 0.01 if abs(obstacle_1.pose.position.x - 1.2) < 0.05 and abs(obstacle_1.pose.position.y + 0) < 0.05: state = 13 elif state == 13: obstacle_1.pose.position.x -= 0.007 obstacle_1.pose.position.y += 0.005 if abs(obstacle_1.pose.position.x - 0.5) < 0.05 and abs(obstacle_1.pose.position.y - 0.5) < 0.05: state = 14 elif state == 14: obstacle_1.pose.position.x += 0.017 obstacle_1.pose.position.y += 0.008 if abs(obstacle_1.pose.position.x - 2.2) < 0.05 and abs(obstacle_1.pose.position.y - 1.3) < 0.05: state = 15 elif state == 15: obstacle_1.pose.position.x -= 0.002 obstacle_1.pose.position.y += 0.007 if abs(obstacle_1.pose.position.x - 2) < 0.05 and abs( obstacle_1.pose.position.y - 2) < 0.05: state = 0 self.pub_model.publish(obstacle_1) time.sleep(0.1)
def __init__(self, queue_url, aws_region='us-east-1', race_duration=180, number_of_trials=3, number_of_resets=10000, penalty_seconds=2.0, off_track_penalty=2.0, collision_penalty=5.0, is_continuous=False, race_type="TIME_TRIAL"): # constructor arguments self._model_updater = ModelUpdater.get_instance() self._deepracer_path = rospkg.RosPack().get_path( DeepRacerPackages.DEEPRACER_SIMULATION_ENVIRONMENT) body_shell_path = os.path.join(self._deepracer_path, "meshes", "f1") self._valid_body_shells = \ set(".".join(f.split(".")[:-1]) for f in os.listdir(body_shell_path) if os.path.isfile( os.path.join(body_shell_path, f))) self._valid_body_shells.add(const.BodyShellType.DEFAULT.value) self._valid_car_colors = set(e.value for e in const.CarColorType if "f1" not in e.value) self._num_sectors = int(rospy.get_param("NUM_SECTORS", "3")) self._queue_url = queue_url self._region = aws_region self._number_of_trials = number_of_trials self._number_of_resets = number_of_resets self._penalty_seconds = penalty_seconds self._off_track_penalty = off_track_penalty self._collision_penalty = collision_penalty self._is_continuous = is_continuous self._race_type = race_type self._is_save_simtrace_enabled = False self._is_save_mp4_enabled = False self._is_event_end = False self._done_condition = any self._race_duration = race_duration self._enable_domain_randomization = False # sqs client # The boto client errors out after polling for 1 hour. self._sqs_client = SQSClient(queue_url=self._queue_url, region_name=self._region, max_num_of_msg=MAX_NUM_OF_SQS_MESSAGE, wait_time_sec=SQS_WAIT_TIME_SEC, session=refreshed_session(self._region)) self._s3_client = S3Client(region_name=self._region) # tracking current state information self._track_data = TrackData.get_instance() self._start_lane = self._track_data.center_line # keep track of the racer specific info, e.g. s3 locations, alias, car color etc. self._current_racer = None # keep track of the current race car we are using. It is always "racecar". car_model_state = ModelState() car_model_state.model_name = "racecar" self._current_car_model_state = car_model_state self._last_body_shell_type = None self._last_sensors = None self._racecar_model = AgentModel() # keep track of the current control agent we are using self._current_agent = None # keep track of the current control graph manager self._current_graph_manager = None # Keep track of previous model's name self._prev_model_name = None self._hide_position_idx = 0 self._hide_positions = get_hide_positions(race_car_num=1) self._run_phase_subject = RunPhaseSubject() self._simtrace_video_s3_writers = [] self._local_model_directory = './checkpoint' # virtual event only have single agent, so set agent_name to "agent" self._agent_name = "agent" # camera manager self._camera_manager = CameraManager.get_instance() # setting up virtual event top and follow camera in CameraManager # virtual event configure camera does not need to wait for car to spawm because # follow car camera is not tracking any car initially self._main_cameras, self._sub_camera = configure_camera( namespaces=[VIRTUAL_EVENT], is_wait_for_model=False) self._spawn_cameras() # pop out all cameras after configuration to prevent camera from moving self._camera_manager.pop(namespace=VIRTUAL_EVENT) dummy_metrics_s3_config = { MetricsS3Keys.METRICS_BUCKET.value: "dummy-bucket", MetricsS3Keys.METRICS_KEY.value: "dummy-key", MetricsS3Keys.REGION.value: self._region } self._eval_metrics = EvalMetrics( agent_name=self._agent_name, s3_dict_metrics=dummy_metrics_s3_config, is_continuous=self._is_continuous, pause_time_before_start=PAUSE_TIME_BEFORE_START) # upload a default best sector time for all sectors with time inf for each sector # if there is not best sector time existed in s3 # use the s3 bucket and prefix for yaml file stored as environment variable because # here is SimApp use only. For virtual event there is no s3 bucket and prefix past # through yaml file. All are past through sqs. For simplicity, reuse the yaml s3 bucket # and prefix environment variable. virtual_event_best_sector_time = VirtualEventBestSectorTime( bucket=os.environ.get("YAML_S3_BUCKET", ''), s3_key=get_s3_key(os.environ.get("YAML_S3_PREFIX", ''), SECTOR_TIME_S3_POSTFIX), region_name=os.environ.get("APP_REGION", "us-east-1"), local_path=SECTOR_TIME_LOCAL_PATH) response = virtual_event_best_sector_time.list() # this is used to handle situation such as robomaker job crash, so the next robomaker job # can catch the best sector time left over from crashed job if "Contents" not in response: virtual_event_best_sector_time.persist( body=json.dumps({ SECTOR_X_FORMAT.format(idx + 1): float("inf") for idx in range(self._num_sectors) }), s3_kms_extra_args=utils.get_s3_kms_extra_args()) # ROS service to indicate all the robomaker markov packages are ready for consumption signal_robomaker_markov_package_ready() PhaseObserver('/agent/training_phase', self._run_phase_subject) # setup mp4 services self._setup_mp4_services()
def reset(self): # rospy.wait_for_service('/gazebo/reset_world') # Reset simulation was causing problems, do not reset simulation # try: # #reset_proxy.call() # self.reset_proxy() # except (rospy.ServiceException) as e: # print ("/gazebo/reset_world service call failed") # rospy.wait_for_service('/iri_wam/controller_manager/list_controllers') # try: # readController = rospy.ServiceProxy('/iri_wam/controller_manager/list_controllers', Empty) # control = readController() # #print (control) # #onHai = bool(controllers.iri_wam_controller.state == "stopped") # except (rospy.ServiceException) as e: # print ("Service call failed: %s"%e) #if not onHai: # rospy.wait_for_service('/iri_wam/controller_manager/switch_controller') # try: # change_controller = rospy.ServiceProxy('/iri_wam/controller_manager/switch_controller', SwitchController) # ret = change_controller(['joint1_position_controller', 'joint2_position_controller', 'joint3_position_controller', 'joint4_position_controller', 'joint5_position_controller', 'joint6_position_controller', 'joint7_position_controller'], ['iri_wam_controller'], 2) # except (rospy.ServiceException) as e: # print ("Service call failed: %s"%e) for joint in range(len(self.publishers)): self.publishers[joint].publish( self.home[joint]) #homing at every reset self.open_gripper_func() time.sleep(self.homingTime) rospy.wait_for_service('/gazebo/set_model_state') try: pose = Pose() state = ModelState() state.model_name = self.objectName['obj1'] pose.position.x = 0.5001911647282589 pose.position.y = 0.1004797189877992 pose.position.z = 0.9 pose.orientation.x = 0.00470048637345294 pose.orientation.y = 0.99998892605584 pose.orientation.z = 9.419015715062839e-06 pose.orientation.w = -0.00023044483691539005 state.pose = pose ret = self.set_state(state) except (rospy.ServiceException) as e: print("/gazebo/set model pose service call failed") _, _ = self.get_object_pose(self.objectName['obj1']) self.objInitial = self.object self.objInitialJS = self.getInverseKinematics( self.objInitial) # reference for sampling the goal self.goal = self.sample_goal_onTable().copy( ) # get a random goal every time you reset obs = self._get_obs() rospy.wait_for_service('/gazebo/set_model_state') try: pose = Pose() state = ModelState() state.model_name = self.objectName['objFixed'] pose.position.x = self.goal[0] pose.position.y = self.goal[1] pose.position.z = 0.9 pose.orientation.x = 0.00470048637345294 pose.orientation.y = 0.99998892605584 pose.orientation.z = 9.419015715062839e-06 pose.orientation.w = -0.00023044483691539005 state.pose = pose ret = self.set_state(state) except (rospy.ServiceException) as e: print("/gazebo/set model pose service call failed") return obs
import utils #for teleop '''EXPLICIT LOCATIONS USED''' RED = [0.701716, 0.400784, 0.83095] BLUE = [0.701716, 0.000784, 0.83095] GREEN = [0.701716, -0.400784, 0.83095] TARGET_GREEN = [0.6, -0.405, 0.82] ALIGN_GRASP_GREEN = [0.7, -0.405, 0.82] RAISED_GREEN = [0.7, -0.405, 1.0] RAISED_DEST_GREEN = [0.7, 0.0, 1.0] DEST_GREEN = [0.7, 0.0, 0.83] DEST_GREEN_REMOVED = [0.45, 0.0, 1.0] _cylinder_name = "unit_object_Green" _cylinder_model_state = ModelState() _cylinder_model_state.model_name = _cylinder_name _cylinder_model_state.reference_frame = '' '''OUTPUT FILE TO WRITE TO''' FILENAME = '/home/simon/experiment' def delete_cylinder(): rospy.wait_for_service("/gazebo/delete_model") try: s = rospy.ServiceProxy("gazebo/delete_model", DeleteModel) resp = s(_cylinder_name) except rospy.ServiceException, e: print "error when calling delete_cylinder: %s"%e sys.exit(1)
# print(position) except: time.sleep(1) # probably the publisher not started yet pass if __name__=="__main__": rospy.init_node('human_controller') pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10) print('Human Controller launched') target_linear_vel = 0.0 target_angular_vel = 0.0 control_linear_vel = 0.0 control_angular_vel = 0.0 pose_subscriber = rospy.Subscriber('/gazebo/model_states', ModelStates, update_pose) p1_state = ModelState() p1_state.model_name = 'person_1' rate = rospy.Rate(10) t = 0 while not rospy.is_shutdown(): x_vel = 2*math.sin(t) # x_vel = 2 ang_vel = 0.5 p1_state.twist.linear.x = x_vel p1_state.twist.angular.x = ang_vel pub.publish(p1_state) t+=1 rate.sleep()
def image_callback(self, msg): data = ModelState() data.model_name = 'drone02' drone = data.pose drone_vel = data.twist posX = drone.position.x posY = drone.position.y posZ = drone.position.z velX = drone_vel.linear.x velY = drone_vel.linear.y #print(d01_posZ) image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8') #bgr8 image = image[:, 30:image.shape[1]-30, :] hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) lower_flood = np.array([0, 100, 100])#np.array([100, 100, 0]) #yellow #np.array([0, 100, 100]) upper_flood = np.array([150, 255, 255])#np.array([255, 255, 120]) #yellow #np.array([100, 255, 255]) lower_ground = np.array([0, 0, 0]) upper_ground = np.array([110, 110, 110]) mask_flood = cv2.inRange(hsv, lower_flood, upper_flood) mask_ground = cv2.inRange(hsv, lower_ground, upper_ground) h, w, d = image.shape #print(h,w) search_top = 0#3*h/4 search_bot = h#3*h/4 + 20 mask_flood[0:search_top, 0:w] = 0 mask_flood[search_bot:h, 0:w] = 0 mask_ground[0:search_top, 0:w] = 0 mask_ground[search_bot:h, 0:w] = 0 M_flood = cv2.moments(mask_flood) M_ground = cv2.moments(mask_ground) if M_flood['m00'] > 0 and M_ground['m00'] > 0: print('fine') cx_flood = int(M_flood['m10']/M_flood['m00']) cy_flood = int(M_flood['m01']/M_flood['m00']) cv2.circle(image, (cx_flood, cy_flood), 10, (0,0,255), -1) cx_ground = int(M_ground['m10']/M_ground['m00']) cy_ground = int(M_ground['m01']/M_ground['m00']) cv2.circle(image, (cx_ground, cy_ground), 10, (255,0,0), -1) cx_middle = min(cx_flood, cx_ground) + int((max(cx_flood, cx_ground)-min(cx_flood, cx_ground))/2) cy_middle = min(cy_flood, cy_ground) + int((max(cy_flood, cy_ground)-min(cy_flood, cy_ground))/2) cv2.circle(image, (cx_middle, cy_middle), 10, (0,0,0), -1) cv2.line(image, (cx_flood, cy_flood), (cx_ground, cy_ground), (0,255,0), 1) # BEGIN CONTROL err_x = cx_middle - w/2 err_y = cy_middle - h/2 #print("err_x: ", err_x, "err_y: ", err_y) self.twist.linear.x = -float(err_y)/300.0 - 1.0 self.twist.linear.y = -float(err_x)/85.0 self.twist.linear.z = 0 #-1.1 #vel_x = -float(err_y)/300.0 + 1.0 #vel_y = -float(err_x)/85.0 self.twist.angular.z = -float(math.atan2((cy_flood - cy_ground),(cx_flood - cx_ground)))*1.8 self.cmd_vel_pub.publish(self.twist) elif M_flood['m00'] > 0 and M_ground['m00'] <= 0 and M_ground['m10'] <= 0: print('lost_ground') cx_flood = int(M_flood['m10']/M_flood['m00']) cy_flood = int(M_flood['m01']/M_flood['m00']) cv2.circle(image, (cx_flood, cy_flood), 10, (0,0,255), -1) self.twist.linear.x = 0.0 self.twist.linear.y = 0.0 self.twist.linear.z = 0.8 self.cmd_vel_pub.publish(self.twist) elif M_flood['m00'] <= 0 and M_flood['m10'] <= 0 and M_ground['m00'] > 0: print('lost_flood') cx_ground = int(M_ground['m10']/M_ground['m00']) cy_ground = int(M_ground['m01']/M_ground['m00']) cv2.circle(image, (cx_ground, cy_ground), 10, (255,0,0), -1) self.twist.linear.x = 0.0 self.twist.linear.y = 0.0 self.twist.linear.z = 0.8 self.cmd_vel_pub.publish(self.twist) else: self.twist.linear.x = 0.0 self.twist.linear.y = 0.0 self.twist.linear.z = 0.0 self.twist.angular.z = 0.0 self.cmd_vel_pub.publish(self.twist) print("else") #cv2.imshow("mask_flood",mask_flood) #cv2.imshow("mask_ground", mask_ground) cv2.imshow("d2", image) cv2.waitKey(3)
def set_target(self): self.goal_pose.position.x = random.uniform(-3.6, 3.6) self.goal_pose.position.y = random.uniform(-3.6, 3.6) target_state = ModelState(model_name="target", pose=self.goal_pose) self.set_model_state(target_state)
turn = turn * speedBindings[key][1] count = 0 print vels(speed, turn) if status == 14: print msg status = (status + 1) % 15 elif key == 'k': x = 0 th = 0 control_speed = 0 control_turn = 0 elif key == ' ': # move human up stairs old_state = get_state_client.call("human_movable", "world") new_state = ModelState() new_state.model_name = "human_movable" new_state.reference_frame = "world" new_state.pose = old_state.pose new_state.twist = old_state.twist new_state.pose.position.x -= 0.5 new_state.pose.position.z += 0.15 set_state_client.call(new_state) else: count += 1 if count > 4: x = 0 th = 0 if key == '\x03':
def __init__(self): """ This Task Env is designed for having the TurtleBot2 in some kind of maze. It will learn how to move around the maze without crashing. """ # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None) assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ "/src;cd " + ros_ws_abspath + ";catkin_make" ROSLauncher(rospackage_name="turtlebot_gazebo", launch_file_name="start_goal_world.launch", ros_ws_abspath=ros_ws_abspath) # Load Params from the desired Yaml file LoadYamlFileParamsTest( rospackage_name="openai_ros", rel_path_from_package_to_file= "src/openai_ros/task_envs/turtlebot2/config", yaml_file_name="turtlebot2_goal_continuous_humanmodel.yaml") # Here we will add any init functions prior to starting the MyRobotEnv super(TurtleBot2HumanModelEnv, self).__init__(ros_ws_abspath) # Only variable needed to be set here high = numpy.array([1, 1, 1, 1]) low = numpy.array([-1, -1, -1, -1]) #high = numpy.array([0,1,1,1]) #low = numpy.array([0,-1,-1,-1]) self.action_space = spaces.Box(low, high) # We set the reward range, which is not compulsory but here we do it. self.reward_range = (-numpy.inf, numpy.inf) self.success = True self.beta = 0 #self.beta = 1 #number_observations = rospy.get_param('/turtlebot2/n_observations') """ We set the Observation space for the 6 observations cube_observations = [ round(current_disk_roll_vel, 0), round(y_distance, 1), round(roll, 1), round(pitch, 1), round(y_linear_speed,1), round(yaw, 1), ] """ # Actions and Observations self.init_linear_forward_speed = rospy.get_param( '/turtlebot2/init_linear_forward_speed') self.init_linear_turn_speed = rospy.get_param( '/turtlebot2/init_linear_turn_speed') self.new_ranges = 180 self.min_range = rospy.get_param('/turtlebot2/min_range') self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value') self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value') # Get Desired Point to Get self.desired_point = Point() self.desired_point.x = rospy.get_param("/turtlebot2/desired_pose/x") self.desired_point.y = rospy.get_param("/turtlebot2/desired_pose/y") self.desired_point.z = rospy.get_param("/turtlebot2/desired_pose/z") self.state_msg = ModelState() self.state_msg.model_name = 'mobile_base' self.state_msg.pose.position.x = 0 self.state_msg.pose.position.y = 0 self.state_msg.pose.position.z = 0 self.state_msg.pose.orientation.x = 0 self.state_msg.pose.orientation.y = 0 self.state_msg.pose.orientation.z = 0 self.state_msg.pose.orientation.w = 0 # We create two arrays based on the binary values that will be assigned # In the discretization method. laser_scan = self.get_laser_scan() #print("lidar data:", laser_scan) rospy.logdebug("laser_scan len===>" + str(len(laser_scan.ranges))) #high = numpy.array([0.5,1,1,1,1,1,6,3.14])#,numpy.array([12,6,3.14,1,3.14,0.5,1]),6*numpy.ones([self.new_ranges]),numpy.array([12,6,3.14,1,3.14,0.5,1]),6*numpy.ones([self.new_ranges]))) high = numpy.hstack( (numpy.array([0.5, 1, 0.5, 1]), 6 * numpy.ones([self.new_ranges]))) #high = numpy.hstack((numpy.array([0,1,0.5,1]),6*numpy.ones([self.new_ranges]))) #high = numpy.hstack((numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]))) #low = numpy.array([-0.5,-1,-1,-1,-1,-1, 0,-3.14])#,numpy.array([-1,-1*6,-1*3.14,-1,-3.14,-0.5,-1]),numpy.zeros([self.new_ranges]),numpy.array([-1,-1*6,-1*3.14,-1,-3.14,-0.5,-1]),numpy.zeros([self.new_ranges]))) low = numpy.hstack((numpy.array([-0.5, -1, -0.5, -1]), numpy.zeros([self.new_ranges]))) #low = numpy.hstack((numpy.array([0,-1,-0.5,-1]),numpy.zeros([self.new_ranges]))) #low = numpy.hstack((numpy.array([-1,-1]),numpy.zeros([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]))) # We only use two integers self.observation_space = spaces.Box(low, high) rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space)) rospy.logdebug("OBSERVATION SPACES TYPE===>" + str(self.observation_space)) # Rewards self.forwards_reward = rospy.get_param("/turtlebot2/forwards_reward") self.turn_reward = rospy.get_param("/turtlebot2/turn_reward") self.end_episode_points = rospy.get_param( "/turtlebot2/end_episode_points") self.cumulated_steps = 0.0 ############################## goal ############################################## self.goal_position = Pose() self.f = open( '/home/i2rlab/shahil_files/shahil_RL_ws_new/src/turtlebot/turtlebot_gazebo/worlds/goal/model.sdf', 'r') self.sdff = self.f.read() ############################## Obstacle ########################################## self.angle = numpy.linspace(-179, 179, 180) / 180 * numpy.pi self.cos = numpy.cos(self.angle) self.sin = numpy.sin(self.angle) ############################## Human Model ###################################### config = tf.ConfigProto(gpu_options=tf.GPUOptions(allow_growth=True)) self.sess = tf.Session(config=config) self.S1 = tf.placeholder(tf.float32, [None, 5], 'S1') #self.S2 = tf.placeholder(tf.float32, [None, 198, 1], 'S2') self.S2 = tf.placeholder(tf.float32, [None, 180], 'S2') self.keep_prob = tf.placeholder(tf.float32) self.a_predict = self.build_c(self.S1, self.S2, self.keep_prob) self.loader() self.goal_space() self.time_start = 0
#!/usr/bin/env python import rospy import tf from std_msgs.msg import Float64 from sensor_msgs.msg import JointState from gazebo_msgs.msg import ModelState from gazebo_msgs.srv import SetModelState rospy.init_node('box') pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=5) quaternion = tf.transformations.quaternion_from_euler(0.000338, 0.003608, 0.000875) msg = ModelState() msg.model_name = 'robot1' msg.pose.position.x = -2.873680 msg.pose.position.y = -0.128694 msg.pose.position.z = 0.012142 msg.pose.orientation.x = quaternion[0] msg.pose.orientation.y = quaternion[1] msg.pose.orientation.z = quaternion[2] msg.pose.orientation.w = quaternion[3] msg.reference_frame = 'world' rate = rospy.Rate(50) try: set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(msg) except rospy.ServiceException, e: print "Service call failed: %s" % e
def run(self, args): commands = args.commands # Split into commands and arguments, every other index arguments = commands[1::2] commands = commands[0::2] for i in xrange(len(commands)): command = commands[i] argument = arguments[i] self.send_feedback("Waiting for odom...") yield self.tx_pose() self.send_feedback("Odom found!") action_kwargs = { 'speed': float(args.speed), 'blind': bool(args.blind) } if command == 'custom': # Let the user input custom commands, the eval may be dangerous # so do away with that at some point. self.send_feedback( "Moving with the command: {}".format(argument)) res = yield eval("self.move.{}.go()".format( argument, **action_kwargs)) elif command in ['tp', 'teleport']: try: rospack = rospkg.RosPack() state_set_pub = yield self.nh.advertise( 'gazebo/set_model_state', ModelState) config_file = os.path.join(rospack.get_path('sub8_gazebo'), 'config', 'teleport_locs.yaml') f = yaml.load(open(config_file, 'r')) if len(arguments) > 1: # Command only takes in one string so to prevent this # command from flowing over into movement we break # before it proceeds. self.send_feedback( "Error, more than one argument detected.") break else: try: x = float(argument.split(' ')[0]) y = float(argument.split(' ')[1]) z = float(argument.split(' ')[2]) # Assumption is if we make it this far, we have successfully # bound the previous three coordinates. # The below would fail if we entered a location name instead of coords # but we should have caught by this point. # This is to catch anything over 3 coordinates. If # only two were given then we would also error out # above. if len(argument.split(' ')) != 3: self.send_feedback( "Incorrect number of coordinates") break except IndexError: self.send_feedback( "Incorrect number of coordinates") break except ValueError: try: if argument in ('list', 'l'): self.send_feedback( 'Available TP locations:') for k in f: print ' * ' + k break argz = f[argument] x = float(argz.split(' ')[0]) y = float(argz.split(' ')[1]) z = float(argz.split(' ')[2]) except LookupError: # This means we did not find the saved location # referenced by the argument. self.send_feedback( "TP location not found, check input.") break modelstate = ModelState( model_name='sub8', pose=Pose(position=Point(x, y, z), orientation=Quaternion(0, 0, 0, 0)), twist=Twist(linear=Point(0, 0, 0), angular=Point(0, 0, 0)), reference_frame='world') # Sometimes you need to sleep in order to get the thing to publish # Apparently there is a latency when you set a publisher and it needs to actually hook into it. # As an additional note, given the way we do trajectory in the sim, we must kill sub to prevent # the trajectory from overriding our teleport and # bringing it back to its expected position. ab = TxAlarmBroadcaster(self.nh, 'kill', node_name='kill') yield ab.raise_alarm( problem_description='TELEPORTING: KILLING SUB', severity=5) yield self.nh.sleep(1) yield state_set_pub.publish(modelstate) yield self.nh.sleep(1) yield ab.clear_alarm() except KeyboardInterrupt: # Catches a ctrl-c situation and ends the program. Break # will just bring the program to its natural conclusion. break elif command in ['zrp', 'level_off', 'zpr']: self.send_feedback("Zeroing roll and pitch") res = yield self.move.zero_roll_and_pitch().go(**action_kwargs) elif command == "stop": self.send_feedback("Stopping...") if args.zrp: res = yield self.move.forward(0).zero_roll_and_pitch().go( **action_kwargs) else: res = yield self.move.forward(0).go(**action_kwargs) else: # Get the command from shorthand if it's there command = SHORTHAND.get(command, command) movement = getattr(self.move, command) trans_move = command[: 3] != "yaw" and command[: 5] != "pitch" and command[: 4] != "roll" unit = "m" if trans_move else "rad" amount = argument # See if there's a non standard unit at the end of the argument if not argument[-1].isdigit(): last_digit_index = [ i for i, c in enumerate(argument) if c.isdigit() ][-1] + 1 amount = argument[:last_digit_index] unit = argument[last_digit_index:] extra = "and zeroing pitch and roll" if args.zrp else "" self.send_feedback("{}ing {}{} {}".format( command, amount, unit, extra)) bad_unit = UNITS.get(unit, "BAD") if bad_unit == "BAD": self.send_feedback("BAD UNIT") break goal = movement(float(amount) * UNITS.get(unit, 1)) if args.zrp: res = yield goal.zero_roll_and_pitch().go(**action_kwargs) else: res = yield goal.go(**action_kwargs) self.send_feedback("Result: {}".format(res.error))
def main(): global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_ global srv_client_go_to_goal_, srv_client_wall_follower_ global circumnavigate_closest_point_, circumnavigate_starting_point_ global count_loop_, count_state_time_ rospy.init_node('bug1_algorithm') sub_laser = rospy.Subscriber('/m2wr/laserscan', LaserScan, laser_callback) sub_odom = rospy.Subscriber('/odom', Odometry, odom_callback) rospy.wait_for_service('/go_to_goal_switch') rospy.wait_for_service('/right_wall_following_switch') rospy.wait_for_service('/gazebo/set_model_state') srv_client_go_to_goal_ = rospy.ServiceProxy('/go_to_goal_switch', SetBool) srv_client_wall_follower_ = rospy.ServiceProxy( '/right_wall_following_switch', SetBool) srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # Set robot position model_state = ModelState() model_state.model_name = 'm2wr' model_state.pose.position.x = initial_position_.x model_state.pose.position.y = initial_position_.y resp = srv_client_set_model_state(model_state) # Initialize going to the point change_state(0) rate = rospy.Rate(20) while not rospy.is_shutdown(): if regions_ == None: continue # 0 - go to point if state_ == 0: if regions_['front'] > 0.15 and regions_['front'] < 1: circumnavigate_closest_point_ = position_ circumnavigate_starting_point_ = position_ change_state(1) # 1 - circumnavigate elif state_ == 1: # If current position is closer to the goal than the previous closest_position, assign current position to closest_point if calc_dist_points( position_, desired_position_) < calc_dist_points( circumnavigate_closest_point_, desired_position_): circumnavigate_closest_point_ = position_ # rospy.loginfo("New closest point update: %s " % circumnavigate_closest_point_) # Compare only after 5 seconds - need some time to get out of starting_point # If robot reaches (is close to) starting point if count_state_time_ > 5 and \ calc_dist_points(position_, circumnavigate_starting_point_) < 0.2: change_state(2) # 2 - go to closest point elif state_ == 2: # If robot reaches (is close to) closest point if calc_dist_points(position_, circumnavigate_closest_point_) < 0.2: change_state(0) count_loop_ = count_loop_ + 1 if count_loop_ == 20: count_state_time_ = count_state_time_ + 1 count_loop_ = 0 rate.sleep()
rospy.wait_for_service('gazebo/pause_physics') pause_physics = rospy.ServiceProxy('gazebo/pause_physics', Empty) rospy.wait_for_service('gazebo/unpause_physics') unpause_physics = rospy.ServiceProxy('gazebo/unpause_physics', Empty) print "got service 'gazebo/set_model_state'" for idx, location in enumerate(tqdm(availableLoc)): for plan in location: pose = plan time_start = time.time() state = ModelState(model_name="sudo-spawn", pose=pose, reference_frame="map") pause_physics() response = setModelState(state) time.sleep(0.1) unpause_physics() time.sleep(0.2) image_data = rospy.wait_for_message( "/sudo/bottom_webcam/image_raw", Image) image = cvBridge.imgmsg_to_cv2(image_data, "bgr8") cv2.imshow("AAAAAARGH", image) cv2.waitKey(1) orientation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) yaw = tf.transformations.euler_from_quaternion(orientation)[2] total_angle = math.degrees(yaw)
if __name__ == "__main__": rospy.init_node("model_dynamics") model = bicycleModel() # fusion = sensorFusion() endList = 0; print(model.waypointList) #wait till a waypoint is received while(not model.waypointList): pass rospy.sleep(7) targetState = ModelState() # targetState = model.waypointList.pop(0) # print("target state", targetState) targetState.pose.position.x = 10 targetState.pose.position.y = 10 safe = True start = time.time() rate = rospy.Rate(100) # 100 Hz while not rospy.is_shutdown(): rate.sleep() # Wait a while before trying to get a new state currState = model.getModelState() if not currState.success: continue
vy = msg.axes[4] if not msg.buttons[6] and msg.buttons[7]: vz = 1 elif msg.buttons[6] and not msg.buttons[7]: vz = -1 else: vz = 0 rospy.init_node('joystick', anonymous=True) swarm_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1000) rospy.Subscriber('/joy', Joy, joy_callback) swarm_msg = [ModelState() for _ in xrange(25)] for i in xrange(25): swarm_msg[i].model_name = 'quadrotor' + str(i) dt = 0.1 x = 0 y = 0 z = 0 while not rospy.is_shutdown(): x = x + vx * dt y = y + vy * dt z = z + vz * dt q = quaternion_from_euler(-0.25 * vy, 0.25 * vx, 0)
def respawn(self): self.counter=0 while not rospy.is_shutdown(): self.spawn_publisher = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size=1) self.counter+=1 rospy.sleep(0.0002) print self.is_episode_finished() print self.linear_velocity,self.counter print self.angular_velocity # print counter if self.counter<20000: posex = self.odom_data[0][0] posey = self.odom_data[0][1] posez = self.odom_data[0][2] poseox = self.odom_data[1][0] poseoy = self.odom_data[1][1] poseoz = self.odom_data[1][2] poseow = self.odom_data[1][3] twistx = self.linear_velocity[0] twisty = self.linear_velocity[1] twistz = self.linear_velocity[2] twistax = self.angular_velocity[0] twistay = self.angular_velocity[1] twistaz = self.angular_velocity[2] if self.counter>20000: if 20000<=self.counter<24000: posex = self.odom_data[0][0] posey = self.odom_data[0][1] posez = self.odom_data[0][2] poseox = self.odom_data[1][0] poseoy = self.odom_data[1][1] poseoz = self.odom_data[1][2] poseow = self.odom_data[1][3] twistx = self.twist_data[0][0] twisty = self.twist_data[0][1] twistz = self.twist_data[0][2] twistax = self.twist_data[1][0] twistay = self.twist_data[1][1] twistaz = self.twist_data[1][2] # print self.state elif 24000<=self.counter<25500: posex = 44.2 posey = -100 posez = 0.3 poseox = 0 poseoy = 0 poseoz = 0 poseow = 1 twistx = 0 twisty = 0 twistz = 0 twistax = 0 twistay = 0 twistaz = 0 # rospy.loginfo("########### Respawning the car... ###########") if self.counter >= 25500: # self.counter = 0 self.new_episode() states = ModelState() states.model_name = "vehicle" states.twist.linear.x = twistx states.twist.linear.y = twisty states.twist.linear.z = twistz states.twist.angular.x = twistax states.twist.angular.y = twistay states.twist.angular.z = twistaz states.pose.position.x = posex states.pose.position.y = posey states.pose.position.z = posez states.pose.orientation.x = poseox states.pose.orientation.y = poseoy states.pose.orientation.z = poseoz states.pose.orientation.w = poseow self.spawn_publisher.publish(states)
def parse_trajectory(input_file): # Extract the start position of the robot with open(input_file, 'r') as input: lines = input.readlines() start_position = [int(lines[1].split()[1]), int(lines[2].split()[1])] # Used to keep track of the top of a reading chunk reading_num = 4 # read every chunk of data while reading_num < len(lines) - CHUNK_LEN: # Append the heading and distance to the heading_distance_list heading_words = lines[reading_num].split() distance_words = lines[reading_num + 1].split() heading = Float32(heading_words[2]) distance = Float32(distance_words[2]) heading_distance_list.append([heading, distance]) # Reading ground truth data # Assign Pose pose = Pose() # position pose.position.x = Float64(lines[reading_num + 6].split()[1]) pose.position.y = Float64(lines[reading_num + 7].split()[1]) pose.position.z = Float64(lines[reading_num + 8].split()[1]) # orientation pose.orientation.x = Float64(lines[reading_num + 10].split()[1]) pose.orientation.y = Float64(lines[reading_num + 11].split()[1]) pose.orientation.z = Float64(lines[reading_num + 12].split()[1]) pose.orientation.w = Float64(lines[reading_num + 13].split()[1]) # Assign Twist twist = Twist() # linear twist.linear.x = Float64(lines[reading_num + 16].split()[1]) twist.linear.y = Float64(lines[reading_num + 17].split()[1]) twist.linear.z = Float64(lines[reading_num + 18].split()[1]) # angular twist.angular.x = Float64(lines[reading_num + 20].split()[1]) twist.angular.y = Float64(lines[reading_num + 21].split()[1]) twist.angular.z = Float64(lines[reading_num + 22].split()[1]) # Append ground truth to list model_state = ModelState() model_state.pose = pose model_state.twist = twist ground_truth_list.append(model_state) # Append noisy headings and distances to their list noisy_heading_distance_list.append([ Float32(lines[reading_num + 25].split()[1].replace( "[", "").replace(",", "")), Float32(lines[reading_num + 27].split()[1].replace( "[", "").replace(",", "")) ]) # Append scan data to list scan_data = lines[reading_num + 29][11:].split() scan_data_list = TurtleBotScan().ranges for data in scan_data: scan_data_list.append( Float32(data.replace(",", "").replace("]", ""))) scan_list.append(scan_data_list) reading_num = reading_num + 30 return start_position, heading_distance_list, ground_truth_list, noisy_heading_distance_list, scan_list
def publish_gazebo_model_state(self): for i in range(0, len(self.vect_gazebo)): try: position_to_pub = ModelState() # is the mark visible? if not we reach exception # (trans_to_pub, rot_to_pub) = self.listener.lookupTransform( # self.vect_gazebo[i][1], MAP, rospy.Time(0)) # Where is the desired object (trans_to_pub, rot_to_pub) = self.listener.lookupTransform( MAP, self.vect_gazebo[i][0], rospy.Time(0)) # create the special message for gazebo position_to_pub.reference_frame = "world" position_to_pub.pose.position.x = trans_to_pub[0] position_to_pub.pose.position.y = trans_to_pub[1] # TODO : this is to be sure that the table is on the ground # but it can not be generalized because the chair origin # is not on the ground, we have to decide wether to fix # the origin of all objects gazebo on the ground or pass # the Z in a parameter or just the marker info if self.vect_gazebo[i][1] == "table": quat_to_send = rot_to_pub position_to_pub.pose.position.z = 0 elif i == 0: euler = euler_from_quaternion(rot_to_pub) quat_to_send = quaternion_from_euler( math.pi - euler[2], 0, 0) position_to_pub.pose.position.z = trans_to_pub[2] else: quat_to_send = rot_to_pub position_to_pub.pose.position.z = trans_to_pub[2] position_to_pub.pose.orientation.x = quat_to_send[0] position_to_pub.pose.orientation.y = quat_to_send[1] position_to_pub.pose.orientation.z = quat_to_send[2] position_to_pub.pose.orientation.w = quat_to_send[3] if i == 0: position_to_pub.model_name = "/virtual_pepper" self.pub.publish(position_to_pub) (trans_to_pub, rot_to_pub) = self.listener.lookupTransform( MAP, "/base_link", rospy.Time(0)) # self.broadcaster.sendTransform( # (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), # "/map", "/world") self.broadcaster.sendTransform( trans_to_pub, rot_to_pub, rospy.Time.now(), "/virtual_pepper/base_link", MAP) else: position_to_pub.model_name = self.vect_gazebo[i][0] self.pub_obj.publish(position_to_pub) except Exception, exc: print"publish gazebo", exc
rospy.set_param('/evaluate',evaluate) # 4.2.1b Reset environment ==> causes gt_node to freeze for more than a minute... # reset_gazebo_service(EmptyRequest()) # 4.2.1c Change position of drone according to new selected starting position pose=Pose() pose.position.x, pose.position.y, starting_height, yaw = sample_new_position(starting_positions) # pose.position.x, pose.position.y, starting_height, yaw=0,0,1,0 print("[run_script]: x: {0}, y: {1}, z: {2}, yaw:{3}".format(pose.position.x, pose.position.y, starting_height, yaw)) # some yaw to quaternion re-orientation code: pose.orientation.z=np.sin(yaw) pose.orientation.w=np.cos(yaw) pose.position.z = 0.1 model_state = ModelState() model_state.model_name = 'quadrotor' if FLAGS.robot.startswith('drone') else 'turtlebot3_burger' model_state.pose=pose state_request = SetModelStateRequest() state_request.model_state = model_state retvals = model_state_gazebo_service(state_request) rospy.set_param('starting_height', starting_height) print("Changed pose with return values: {0}".format(retvals)) time.sleep(1) #CHANGED unpause_physics_client(EmptyRequest()) else: # 4.2.2 Launch Gazebo again # 4.2.2a Ensure previous Gazebo is not running anymore if gazebo_popen!=None:
def simulate_anomaly(): """ Spawn and move the fire """ # {id: [ initial_pos, speed]} anomalies = {'fogo4': [[-0.50, 0.30], [0.0051, 0.0]], 'fogo5': [[-0.4, -0.30], [0.0051, 0.0]], 'fogo6': [[-0.65, 0.2], [0.0051, 0.0]], 'fogo7': [[0.5, 0], [0.0051, 0.0]], 'fogo8': [[0, 0.4], [0.0051, 0.0]], 'fogo9': [[0, -0.35], [0.0051, 0.0]], 'fogo1': [[0.40, 0.40], [0.0051, 0.0]], 'fogo2': [[-2.30, 0.20], [0.0051, 0.0]], } # {id: initial_position, target, initial time} anomalies = {'fogo4': [[-0.50, 0.30], [5.00, 0.0]], 'fogo5': [[-0.4, -1.60], [5.00, 0.0]], 'fogo6': [[-1.0, -2.02], [5.00, 0.0]], 'fogo7': [[1.0, -0.30], [5.00, 0.0]], 'fogo8': [[0.0, 0.4], [5.00, 0.0]], 'fogo9': [[-1.30, -0.90], [5.00, 0.0]], 'fogo1': [[0.80, -1.60], [5.00, 0.0]], 'fogo2': [[-0.8, 0.0], [5.00, 0.0]], } anomalies = {'fogo4': [[-0.50, 0.30], [.00, 0.0], 0], 'fogo5': [[-0.4, -1.60], [.00, 0.0], 0], 'fogo6': [[-1.0, -2.02], [.00, 0.0], 0], 'fogo7': [[1.0, -0.30], [.00, 0.0], 0], 'fogo8': [[0.0, 0.4], [.00, 0.0], 0], 'fogo9': [[-1.30, -0.90], [.00, 0.0], 0], 'fogo1': [[0.80, -1.60], [.00, 0.0], 0], 'fogo2': [[-0.8, 0.0], [.00, 0.0], 0], } anomalies = {'fogo4': [[-4.0, 3.0], [-4.0, 3.0], 40], 'fogo5': [[1.4, 1.80], [1.4, 1.80], 100], 'fogo6': [[-2.60, -1.2], [-2.60, -1.2], 150], # 'fogo7': [[3.30, -2.40], [3.30, -2.40], 200] } anomalies = {'fogo4': [[-.0, .0], [-.0, .0], 0], } existent_anomalies = Set() expand = rospy.get_param('~expand', False) if expand: for k, a in anomalies.items(): anomalies[k][0], anomalies[k][1] = anomalies[k][1], anomalies[k][0] communicator = Communicator('Robot1') # ## SPAWN # for anom, data in anomalies.items(): # spawn_fire(anom, data[0][0], data[0][1]) # pass # ### MOVE service_name = '/gazebo/set_model_state' rospy.init_node('anomaly_simulator') rospy.wait_for_service(service_name) client_ms = rospy.ServiceProxy(service_name, SetModelState) rospy.sleep(0.1) r = rospy.Rate(2) # 10hz # # # moving_x = 0 log = [] while not rospy.is_shutdown(): for anom, data in anomalies.items(): if data[2] > rospy.get_rostime().secs: continue else: if not anom in existent_anomalies: print "spawn fire ", anom spawn_fire(anom, data[0][0], data[0][1]) existent_anomalies.add(anom) x, y = data[0] target_x, target_y = data[1] angle = math.atan2(target_y - y, target_x - x) v = 0.002 data[0][0] += v * math.cos(angle) data[0][1] += v * math.sin(angle) # Model state ms = ModelState() ms.model_name = anom ms.pose.position.x, ms.pose.position.y = data[0] ms.reference_frame = 'world' # Set new model state set_ms = SetModelState() set_ms.model_state = ms client_ms.call(ms) rospy.sleep(0.05) time = rospy.get_rostime().secs + rospy.get_rostime().nsecs / 1000000000.0 orobots, sensed_points, anomaly_polygons = communicator.read_inbox() log.append((time, anomaly_polygons, copy.deepcopy(anomalies), sensed_points)) with open(LOG_FILE, 'wb') as output: pickle.dump(log, output, pickle.HIGHEST_PROTOCOL) r.sleep()