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 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 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 _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 moving(self): while not rospy.is_shutdown(): # When training in the moving_obstacles module while True: gate = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) for i in range(len(model.name)): if model.name[i] == 'gate_moving' or model.name[ i] == 'gate_moving_1': gate.model_name = model.name[i] gate.pose = model.pose[i] gate.twist = Twist() gate.twist.linear.z = GATE_MOVE_SPEED * self.gate_dir # Gate is moving down and close to bottom, wait "shut" if (model.pose[i].position.z < 0.3 and self.gate_dir == -1): self.gate_dir = 1 gate.twist.linear.z = 0 gate.pose.position.z = 0.25 self.pub_model.publish(gate) try: rospy.sleep(GATE_WAIT_TIME) except rospy.exceptions.ROSTimeMovedBackwardsException, e: print( "Ros error due to reset during sleep, disregard" ) continue # Gate is moving up and close to top, wait "open" if (model.pose[i].position.z > 1 and self.gate_dir == 1): self.gate_dir = -1 gate.twist.linear.z = 0 gate.pose.position.z = 1 self.pub_model.publish(gate) try: rospy.sleep(GATE_WAIT_TIME) except rospy.exceptions.ROSTimeMovedBackwardsException, e: print( "Ros error due to reset during sleep, disregard" ) continue self.pub_model.publish(gate) try: rospy.sleep(0.1) except rospy.exceptions.ROSTimeMovedBackwardsException, e: print( "Ros error due to reset during sleep, disregard" )
def moving_y(self, distance): obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) model_original_pose_y = model.pose[0].position.y for i in range(len(model.name)): if model.name[i] == self.model_name and round((model.pose[i].position.y),1) <= round(self.y_model_pose,1)+1.0 and self.flag1 == 0: obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.y = distance obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif model.name[i] == self.model_name and round(model.pose[i].position.y,1) != round(self.y_model_pose,1): self.flag1 = 1 obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.y = -distance obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif round(model.pose[i].position.y,1) == round(self.y_model_pose,1): self.flag1 = 0
def _reset(self): # print("Resetting ...") rospy.wait_for_service('gazebo/set_model_state') try: state = ModelState() state.model_name = self.name_model state.reference_frame = "world" state.pose = self.turtlebot_state.pose state.twist = self.turtlebot_state.twist self.set_model(state) except rospy.ServiceException, e: print("/gazebo/set_model_state service call failed")
def callback(data): twist = Twist() twist.linear.x = data.linear.x twist.linear.y = data.linear.y twist.linear.z = data.linear.z twist.angular.x = data.angular.x twist.angular.y = data.angular.y twist.angular.z = data.angular.z state = ModelState() state.model_name = "youbot_2" state.twist = twist state.reference_frame = "youbot_2" ret = set_state(state)
def moving_tile_y_alternate_long_path(self): obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) model_original_pose_y = model.pose[0].position.y for i in range(len(model.name)): if model.name[i] == self.model_name and round(model.pose[i].position.y) <= round(self.y_model_pose)+6.0 and round(model.pose[i].position.y) >= -round(self.y_model_pose)+2. and self.flag1 == 0: obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.y = -1. obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif model.name[i] == self.model_name and round(model.pose[i].position.y) != round(self.y_model_pose): self.flag1 = 1 obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.y = 1. obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif round(model.pose[i].position.y) == round(self.y_model_pose): self.flag1 = 0
def modelStatesCb(self, msg): ''' Callback for gazebo model states @param msg: received message @type msg: gazebo_msgs/ModelStates ''' for i in range(len(msg.name)): if msg.name[i] in self._gazebo_robots: model_state = ModelState() model_state.model_name = msg.name[i] model_state.pose = msg.pose[i] model_state.twist = msg.twist[i] self._gazebo_robots[msg.name[i]]['model'].update( model_state, rospy.Time.now()) elif msg.name[i] in self._gazebo_objects: model_state = ModelState() model_state.model_name = msg.name[i] model_state.pose = msg.pose[i] model_state.twist = msg.twist[i] self._gazebo_objects[msg.name[i]]['model'].update( model_state, rospy.Time.now())
def main(): rospy.init_node('set_vel') current_time = rospy.Time.now().to_sec() last_time = rospy.Time.now().to_sec() x = 5 y = 0 theta = 0 rate = rospy.Rate(5) rospy.Subscriber('cmd_velBox', TwistStamped, callback) while not rospy.is_shutdown(): current_time = rospy.Time.now().to_sec() dt = (current_time - last_time) delta_x = lin_x.data * dt delta_y = lin_y.data * dt delta_theta = ang_z.data * dt x += delta_x y += delta_y theta += delta_theta q = theta_to_quaternion(theta) modelstate = ModelState() modelstate.model_name = 'simple_box' modelstate.reference_frame = "world" model_twist = Twist() model_twist.linear.x = 0 model_twist.linear.y = 0 model_twist.linear.z = 0 model_twist.angular.x = 0 model_twist.angular.y = 0.0 model_twist.angular.z = 0.0 model_pose = Pose() model_pose.position.x = x model_pose.position.y = y model_pose.position.z = 0 model_pose.orientation.x = q.x model_pose.orientation.y = q.y model_pose.orientation.z = q.z model_pose.orientation.w = q.w modelstate.twist = model_twist modelstate.pose = model_pose last_time = rospy.Time.now().to_sec() rate.sleep() rospy.wait_for_service('/gazebo/set_model_state') try: set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(modelstate) except rospy.ServiceException, e: print "Service call failed: %s" % e
def try_to_pick_up_apple(self): """Try to pickup apple.""" is_apple_picked = False # idicate wether picked apple or not try: distance_min, number_min = self.closest_apple() rospy.logdebug( "Trying to pick up apple - distance %.2f, " "minimum to succeed %.2f", distance_min, self.apple_distance) if distance_min <= self.apple_distance and self.apple_idx_list: # Instead of deleting apples from world we change it's position # by setting them far away from environment new_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) model_state = ModelState() apple_name = 'cricket_ball_' + str(number_min) rospy.loginfo('Picked %s', apple_name) model_state.model_name = apple_name twist = Twist() twist = self._set_twist(twist) model_state.twist = twist pose = Pose() pose.position.x = 99 pose.position.y = -99 pose.position.z = 0.0 pose.orientation.x = 0.0 pose.orientation.y = 0.0 pose.orientation.z = 0.0 pose.orientation.w = 0.0 model_state.pose = pose model_state.reference_frame = 'world' new_model_state(model_state) # We need to keep up a list of pick-up apples because we dont actually delete # instead change position but we dont want to try pick them up again self.apple_idx_list.remove(number_min) is_apple_picked = True else: rospy.logdebug( 'Come closer to the apple. Min distance is %.2f', self.apple_distance) except rospy.ServiceException as err: rospy.logerr("Get Model State service call failed: %s", err) return is_apple_picked
def talker(): pub = rospy.Publisher('/ackermann_cmd', Float32 speed, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz new_model_state_msg = ModelState() new_model_state_msg.model_name = ackermann_model_name new_model_state_msg.pose = ackermann_pose new_model_state_msg.twist = ackermann_twist publisher.publish(new_model_state_msg)
def moving(self): 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] obstacle_1.twist = model.twist[i] obstacle_1.pose.position.x = random(-20, 20) / 10 obstacle_1.twist.linear.x = random(-20, 20) / 10 self.pub_model.publish(obstacle_1)
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 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 make_a_brave_new_forest(self): # generate random samples random_interval_x = self.spacing_trees_x / 3 offset_x = 7.5 random_interval_y = self.spacing_trees_y offset_y = -int(self.num_trees_y * self.spacing_trees_y / 2) + 3 x = np.linspace( offset_x, offset_x + (self.num_trees_x - 1) * self.spacing_trees_x, self.num_trees_x) y = np.linspace( offset_y, offset_y + (self.num_trees_y - 1) * self.spacing_trees_y, self.num_trees_y) counter = 0 np.random.seed() #use seed from sys time to build new env on reset for i in range(self.num_trees_x): for j in range(self.num_trees_y): name = 'unit_cylinder_' + str(counter) counter += 1 noise_x = np.random.random() - 0.5 noise_x *= random_interval_x noise_y = np.random.random() - 0.5 noise_y *= random_interval_y x_tree = x[i] + noise_x y_tree = y[j] + noise_y model_pose = Pose() model_pose.position.x = x_tree model_pose.position.y = y_tree model_pose.position.z = 5.0 model_pose.orientation.x = 0.0 model_pose.orientation.y = 0.0 model_pose.orientation.z = 0.0 model_pose.orientation.w = 1.0 model_twist = Twist() model_state = ModelState() model_state.model_name = name model_state.pose = model_pose model_state.twist = model_twist model_state.reference_frame = 'world' # change to 'world'? rospy.wait_for_service('/gazebo/set_model_state') try: self.set_model_state_proxy(model_state) except rospy.ServiceException, e: print "Service call failed: %s" % e
def call_service(self, req): """ Move the turtlebot to the requested pose. if collision: return turtlebot to original pose, return """ current_pose = self.get_current_pose() desired_angle = np.clip(req.heading.data, -np.pi, np.pi) desired_distance = np.clip(req.distance.data, 0, 1.5) self.set_heading_angle(desired_angle) current_position = self.get_current_position(current_pose) target_pose = np.zeros((2, )) target_pose[ 0] = current_position[0] + desired_distance * np.cos(desired_angle) target_pose[ 1] = current_position[1] + desired_distance * np.sin(desired_angle) waypoints_x = np.linspace(current_position[0], target_pose[0], self.step_size) waypoints_y = np.linspace(current_position[1], target_pose[1], self.step_size) waypoints = np.vstack([waypoints_x, waypoints_y]).T for i in range(waypoints.shape[0]): self.set_position(waypoints[i]) rospy.sleep(rospy.Duration(0.1)) scan_data = rospy.wait_for_message("/turtlebot_scan", TurtleBotScan) model_state = self.get_model_state(model_name="mobile_base") ground_truth = ModelState() ground_truth.pose = model_state.pose ground_truth.twist = model_state.twist ground_truth.model_name = "mobile_base" noisy_heading = Float32() noisy_heading.data = desired_angle + np.random.normal( 0, self.rotation_noise) noisy_distance = Float32() noisy_distance.data = desired_distance + np.random.normal( 0, self.translation_noise) return TurtleBotControlResponse(ground_truth=ground_truth, noisy_heading=noisy_heading, noisy_distance=noisy_distance, scan_data=scan_data)
def _reset(self): #cv2.destroyAllWindows() # Resets the state of the environment and returns an initial observation. # rospy.wait_for_service('/gazebo/reset_simulation') # try: # #reset_proxy.call() # self.reset_proxy() # except rospy.ServiceException: # print ("/gazebo/reset_simulation service call failed") rospy.wait_for_service('gazebo/set_model_state') state = ModelState() state.model_name = self.name_model state.reference_frame = "world" state.pose= self.turtlebot_state.pose state.twist = self.turtlebot_state.twist self.set_model(state) # self.num_target = np.random.randint(3) self.checked_point = [0] * self.num_hint # self.num_hint = len(self.hint_pos[self.num_target]) # self.setTarget() # Unpause simulation to make observation rospy.wait_for_service('/gazebo/unpause_physics') try: self.unpause() except rospy.ServiceException: print ("/gazebo/unpause_physics service call failed") #read laser data data = None while data is None: try: data = rospy.wait_for_message('/scan', LaserScan, timeout=5) except: pass pos = self.model_state(self.name_model, "world").pose.position done = self.calculate_observation(data, pos) image, laser = self.discretize_observation(data) rospy.wait_for_service('/gazebo/pause_physics') try: #resp_pause = pause.call() self.pause() except rospy.ServiceException: print ("/gazebo/pause_physics service call failed") return np.reshape(np.array([image, np.asarray(laser), np.asarray([pos.x, pos.y], dtype = np.float32)]), [1, 3])
def set_states(**kwargs): # rospy.init_node('model_state_client') rospy.wait_for_service('/gazebo/set_model_state') model_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) for name, pose in kwargs.items(): model_state = ModelState() model_state.model_name = name model_state.pose = Pose(Point(*pose), Quaternion(*(0.0, 0.0, 0.0, 0.0))) model_state.twist = Twist(Vector3(*(0.0, 0.0, 0.0)), Vector3(*(0.0, 0.0, 0.0))) model_state.reference_frame = '' model_state_service(model_state) rospy.sleep(1.0)
def states_callback(self, states_msg): states = states_msg.data model_state_msg = ModelState() model_state_msg.pose = Pose() model_state_msg.twist = Twist() model_state_msg.pose.position.x = states[0] model_state_msg.pose.position.y = states[1] model_state_msg.pose.position.z = 0 model_state_msg.pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, states[2])) model_state_msg.model_name = "unicycle" model_state_msg.reference_frame = "world" self.pub.publish(model_state_msg)
def moving_tile_y_long_path1(self): obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) model_original_pose_y = model.pose[0].position.y for i in range(len(model.name)): if model.name[i] == self.model_name and self.flag1==0 and self.flag2 == 1 and round(model.pose[i].position.y) > round(self.y_model_pose): obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.y = -speed obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif model.name[i] == self.model_name and round(model.pose[i].position.y) <= -3.0: self.flag3 = 1 self.flag2 = 0
def moveGazeboModels(self, command): rospy.wait_for_service('/gazebo/set_model_state') try: move_model = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) for c in command.models: destModelState = ModelState() destModelState.model_name = c.name pose = Pose() copyRosProto(c.pose, pose) destModelState.pose = pose destModelState.twist = Twist() destModelState.reference_frame = 'world' resp = move_model(destModelState) except rospy.ServiceException, e: rospy.logerr("Set model state service call failed: {0}".format(e))
def talker(): pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=100) sub = rospy.Subscriber('/gazebo/model_states', ModelStates, sub_callback) rospy.init_node('husky', anonymous=True) rate = rospy.Rate(10) # 10hz pub_msg = ModelState() while not rospy.is_shutdown(): if data_husky.pose: pub_msg.model_name = 'husky_sim' pub_msg.pose = data_husky.pose pub_msg.twist = data_husky.twist pub_msg.twist.linear.x = pub_msg.twist.linear.x + 0.11 pub.publish(pub_msg) rate.sleep()
def try_to_pick_up_apple(self): reward = 0 try: distanceLim = 0.50 #We should define the minimum distance to apple, where robot can pick up distanceMin, numberMin = self.closest_apple() print( 'Trying to pick up apple - distance %.2f, minimum to succeed %.2f' % (distanceMin, distanceLim)) if distanceMin <= distanceLim: #delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) new_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) model_state = ModelState() apple_name = 'cricket_ball_' + str(numberMin) print('Picked %s' % apple_name) model_state.model_name = apple_name twist = Twist() twist.linear.x = 0 twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = 0 model_state.twist = twist pose = Pose() pose.position.x = 0.2 pose.position.y = -2.4 pose.position.z = 0.0 pose.orientation.x = 0.0 pose.orientation.y = 0.0 pose.orientation.z = 0.0 pose.orientation.w = 0.0 model_state.pose = pose model_state.reference_frame = 'world' #delete_apple = delete_model(apple_name) new_apple_state = new_model_state(model_state) reward = 1000 else: reward = 0 # reward = -1 print( 'You should come closer to the apple. Minimal distance = 0.50' ) except rospy.ServiceException as e: rospy.loginfo( "Get Model State service call failed: {0}".format(e)) return reward
def talker(path): rospy.init_node('talker', anonymous=True) pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10) rate = rospy.Rate(10) # 10hz temp = path x = ModelState() x.model_name = "piano2" x.pose = Pose() x.pose.orientation.y = 0 x.pose.orientation.w = 1.0 x.pose.orientation.x = 0 x.pose.orientation.z = 0 while not rospy.is_shutdown(): x.twist = Twist() x.twist.linear.y = 0 x.twist.angular.y=0 for i in range(0, len(temp)): x.pose.position.x = temp[i][0] x.pose.position.y = temp[i][1] x.pose.position.z = 0.31 #angle_to_turn = atan2 (temp[i+1][1]-temp[i][1], temp[i+1][0]-temp[i+1][0])*math.pi/180 x.pose.orientation.z = random.uniform(0, 1) #x.twist = Twist() #x.twist.linear.x = randint(-60, 60)*math.pi/180 rospy.loginfo(x) pub.publish(x) time.sleep(0.3) # print "one loop done" exit()
def modelStatesCallback(msg): global model_name, model_state_pub, x_vel, y_vel, model_state_sub index_of_interest = -1 for i in range(len(msg.name)): if msg.name[i] == model_name: index_of_interest = i break if index_of_interest >= 0: model_state = ModelState() model_state.model_name = model_name model_state.pose = msg.pose[index_of_interest] twist = Twist() twist.linear.x = x_vel twist.linear.y = y_vel if msg.twist[index_of_interest] != twist: model_state.twist = twist model_state_pub.publish(model_state)
def reset_model(robot, pose): model_state = ModelState() model_state.model_name = robot model_state.pose = copy.deepcopy(pose) if randomise_positions: model_state.pose.position.y += random.random() * 0.5 - 0.25 if model_state.pose.position.x > 0: model_state.pose.position.x -= random.random() * 0.4 - 0.06 else: model_state.pose.position.x += random.random() * 0.4 - 0.06 yaw = 2 * math.acos(model_state.pose.orientation.w) yaw += random.random() * 1.6 - 0.8 model_state.pose.orientation.z = math.sin(yaw/2) model_state.pose.orientation.w = math.cos(yaw/2) model_state.twist = Twist() model_state.reference_frame = "world" set_robot_pos(model_state)
def set_velocity(x, y): state_publisher = Publisher("/gazebo/set_model_state", ModelState, queue_size=10) rospy.init_node("model_state_setter", anonymous=True) new_vel = Twist() new_vel.linear.x = x new_vel.linear.y = y new_state = ModelState() new_state.twist = new_vel new_state.model_name = "robot" new_state.reference_frame = "ground_plane" rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): rospy.loginfo(new_state) state_publisher.publish(new_state) rate.sleep()
def setObject(): objName = 'test_robot' pub = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10) rospy.init_node('setObject', anonymous=True) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): msg = ModelState() resp1 = gms_client(objName, objName) msg = ModelState() msg.pose = resp1.pose msg.twist = resp1.twist msg.pose.position.x += .30 msg.model_name = objName print msg pub.publish(msg) rate.sleep() print "here"
def set_position(x=0, y=0): rospy.init_node("set_pos") curr_state = getModelState() new_state = ModelState() new_state.model_name = 'gem' new_state.pose = curr_state.pose new_state.pose.position.x = x new_state.pose.position.y = y new_state.pose.position.z = 1 q = euler_to_quaternion([np.pi / 2, 0, 0]) new_state.pose.orientation.x = q[0] new_state.pose.orientation.y = q[1] new_state.pose.orientation.z = q[2] new_state.pose.orientation.w = q[3] new_state.twist = curr_state.twist setModelState(new_state)
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 _reset(self): rospy.wait_for_service('gazebo/set_model_state') self.num_target = np.random.randint(3) self.num_hint = len(self.hint_pos[self.num_target]) self.setTarget() state = ModelState() state.model_name = self.name_model state.reference_frame = "world" state.pose = self.turtlebot_state.pose state.twist = self.turtlebot_state.twist self.set_model(state) # Unpause simulation to make observation rospy.wait_for_service('/gazebo/unpause_physics') try: self.unpause() except rospy.ServiceException, e: print("/gazebo/unpause_physics service call failed")
def setmodel(): pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10) rospy.init_node('set_model_state', anonymous=True) rate = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): modelstate = get_state_call("quadrotor_imu") msg = ModelState() msg.pose = modelstate.pose msg.twist = modelstate.twist msg.pose.position.z = 3.0 msg.twist.linear.z = 0 msg.model_name = "quadrotor_imu" pub.publish(msg) rate.sleep()
def teleportRandom(self): ''' Teleport agent return new x and y point return agent posX, posY in list ''' model_state_msg = ModelState() model_state_msg.model_name = self.agent_model_name # maze 1 xy_list = [[-2, 2], [-2, 1], [2, -2], [2, 2], [-1, 2], [-1, 1], [1, -1], [1, 2]] # Get random position for agent pose = Pose() pose.position.x, pose.position.y = random.choice(xy_list) model_state_msg.pose = pose model_state_msg.twist = Twist() model_state_msg.reference_frame = "world" # Start teleporting in Gazebo isTeleportSuccess = False for i in range(5): if not isTeleportSuccess: try: rospy.wait_for_service('/gazebo/set_model_state') telep_model_prox = rospy.ServiceProxy( '/gazebo/set_model_state', SetModelState) telep_model_prox(model_state_msg) isTeleportSuccess = True break except Exception as e: rospy.logfatal("Error when teleporting agent " + str(e)) else: rospy.logwarn("Trying to teleporting agent..." + str(i)) time.sleep(2) if not isTeleportSuccess: rospy.logfatal("Error when teleporting agent") return "Err", "Err" return pose.position.x, pose.position.y
def moving_tile_x_long_path1(self): obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) model_original_pose_x = model.pose[0].position.x for i in range(len(model.name)): if model.name[i] == self.model_name and self.flag==1 and self.flag1 == 0 and self.flag2==0 and self.flag3 == 1 and round(model.pose[i].position.x) < round(self.x_model_pose)+3 : obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.x = speed obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif model.name[i] == self.model_name and round(model.pose[i].position.x) >= 4.0: print("1") self.flag4 = 1 self.flag3 = 0
def movingTo(self, goal_x, goal_y, goal_z): #pub_model = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1) obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) #print(model.name) #print(model.twist[2]) # print(type(obstacle)) #time.sleep(5) for i in range(len(model.name)): if model.name[ i] == 'bot': # the model name is defined in .xacro file obstacle.model_name = 'bot' obstacle.pose = model.pose[i] obstacle.pose.position.x = float(goal_x) obstacle.pose.position.y = float(goal_y) obstacle.twist = Twist() obstacle.twist.angular.z = float(goal_z) self.pub_model.publish(obstacle)
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.")
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': break target_speed = speed * x target_turn = turn * th