def freezeBase(self, flag): #toggle gravity req_reset_gravity = SetPhysicsPropertiesRequest() #ode config req_reset_gravity.time_step = 0.001 req_reset_gravity.max_update_rate = 1000 req_reset_gravity.ode_config.sor_pgs_iters = 50 req_reset_gravity.ode_config.sor_pgs_w = 1.3 req_reset_gravity.ode_config.contact_surface_layer = 0.001 req_reset_gravity.ode_config.contact_max_correcting_vel = 100 req_reset_gravity.ode_config.erp = 0.2 req_reset_gravity.ode_config.max_contacts = 20 if (flag): req_reset_gravity.gravity.z = 0.0 else: req_reset_gravity.gravity.z = -9.81 self.reset_gravity(req_reset_gravity) # create the message req_reset_world = SetModelStateRequest() #create model state model_state = ModelState() model_state.model_name = "hyq" model_state.pose.position.x = 0.0 model_state.pose.position.y = 0.0 model_state.pose.position.z = 0.8 model_state.pose.orientation.w = 1.0 model_state.pose.orientation.x = 0.0 model_state.pose.orientation.y = 0.0 model_state.pose.orientation.z = 0.0 model_state.twist.linear.x = 0.0 model_state.twist.linear.y = 0.0 model_state.twist.linear.z = 0.0 model_state.twist.angular.x = 0.0 model_state.twist.angular.y = 0.0 model_state.twist.angular.z = 0.0 req_reset_world.model_state = model_state # send request and get response (in this case none) self.reset_world(req_reset_world)
def move_on_line(self, start, goal): dx = goal[0] - start[0] dy = goal[1] - start[1] segment_length = math.sqrt(dx**2 + dy**2) segment_time = segment_length / self.vel yaw = math.atan2(dy, dx) segment_step_count = int(segment_time * self.node_frequency) if segment_step_count == 0: return segment_time = segment_length / self.vel / segment_step_count for step in numpy.linspace(0, segment_length, segment_step_count): step_x = start[0] + step * math.cos(yaw) step_y = start[1] + step * math.sin(yaw) object_new_pose = Pose() object_new_pose.position.x = step_x object_new_pose.position.y = step_y quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw) object_new_pose.orientation.x = quat[0] object_new_pose.orientation.y = quat[1] object_new_pose.orientation.z = quat[2] object_new_pose.orientation.w = quat[3] # spawn new model model_state = ModelState() model_state.model_name = self.name model_state.pose = object_new_pose model_state.reference_frame = 'world' # publish message self.pub.publish(model_state) # call service req = SetModelStateRequest() req.model_state = model_state #res = self.srv_set_model_state(req) #if not res.success: # print "something went wrong in service call" # sleep until next step self.rate.sleep()
def talker(): reset_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10) wp_pub = rospy.Publisher('/mybot/set_goal', Vector3, queue_size=10) rospy.init_node('gazebo_reset', anonymous=True) rate = rospy.Rate(10) # 10hz reset_msg = ModelState() reset_msg.model_name = 'mybot' reset_msg.pose.position.x = 0 reset_msg.pose.position.y = 0 reset_msg.pose.position.z = 0 reset_msg.pose.orientation.x = 0 reset_msg.pose.orientation.y = 0 reset_msg.pose.orientation.z = 0 reset_msg.pose.orientation.w = 0 reset_msg.twist.linear.x = 0 reset_msg.twist.linear.y = 0 reset_msg.twist.linear.z = 0 reset_msg.twist.angular.x = 0 reset_msg.twist.angular.y = 0 reset_msg.twist.angular.z = 0 reset_msg.reference_frame = '' wp_msg = Vector3() wp_msg.x = 0 wp_msg.y = 0 wp_msg.z = 0 while not rospy.is_shutdown(): # hello_str = "hello world %s" % rospy.get_time() # rospy.loginfo(hello_str) # pub.publish(hello_str) #while t1-t0<=1: reset_pub.publish(reset_msg) wp_pub.publish(wp_msg) rate.sleep()
def rover_reset(self): # Reset Rover-related Episodic variables rospy.wait_for_service('gazebo/set_model_state') self.x = INITIAL_POS_X self.y = INITIAL_POS_Y # Put the Rover at the initial position model_state = ModelState() model_state.pose.position.x = INITIAL_POS_X model_state.pose.position.y = INITIAL_POS_Y model_state.pose.position.z = INITIAL_POS_Z model_state.pose.orientation.x = INITIAL_ORIENT_X model_state.pose.orientation.y = INITIAL_ORIENT_Y model_state.pose.orientation.z = INITIAL_ORIENT_Z model_state.pose.orientation.w = INITIAL_ORIENT_W model_state.twist.linear.x = 0 model_state.twist.linear.y = 0 model_state.twist.linear.z = 0 model_state.twist.angular.x = 0 model_state.twist.angular.y = 0 model_state.twist.angular.z = 0 model_state.model_name = 'rover' self.gazebo_model_state_service(model_state) self.last_collision_threshold = sys.maxsize self.last_position_x = self.x self.last_position_y = self.y time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND) self.distance_travelled = 0 self.current_distance_to_checkpoint = INITIAL_DISTANCE_TO_CHECKPOINT self.steps = 0 self.reward_in_episode = 0 self.collision = False self.closer_to_checkpoint = False self.power_supply_range = MAX_STEPS self.reached_midpoint = False # First clear the queue so that we set the state to the start image _ = self.image_queue.get(block=True, timeout=None) self.set_next_state()
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 __init__(self, car_id, start_dist, speed, lanes_list_np, change_lane_freq_sec=0): # Wait for ros services rospy.wait_for_service('/gazebo/set_model_state') rospy.wait_for_service('/gazebo/spawn_urdf_model') self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) self.spawn_model_client = rospy.ServiceProxy('gazebo/spawn_urdf_model', SpawnModel) # Spawn the new car robot_description = rospy.get_param('robot_description_bot') car_initial_pose = Pose() car_initial_pose.position.x = 0 car_initial_pose.position.y = 0 car_initial_pose.position.z = 0 self.car_name = "racecar_{}".format(car_id) self.resp = self.spawn_model_client(self.car_name, robot_description, '/{}'.format(self.car_name), car_initial_pose, '') logger.info('Spawn status: {}'.format(self.resp.status_message)) # Initialize member data and reset car self.car_speed = speed # Initializing the lane that the bot car runs self.lanes_list_np = lanes_list_np self.lanes_one_hot = np.zeros(len(lanes_list_np), dtype=bool) self.lanes_one_hot[0] = True self.shapely_lane = lanes_list_np[self.lanes_one_hot][0] print('self.shapely_lane.length = ', self.shapely_lane.length) # Change the lanes after certain time self.change_lane_freq_sec = change_lane_freq_sec self.reverse_dir = False self.car_model_state = ModelState() self.reset_sim(start_dist)
def _get_car_start_model_state(self, start_dist): '''get avaliable car start model state when starting a new lap Args: start_dist (float): start distance Returns: ModelState: start state ''' _, closest_object_pose = self._get_closest_obj(start_dist) # Compute the start pose start_pose = self._track_data_.center_line.interpolate_pose( start_dist, finite_difference=FiniteDifference.FORWARD_DIFFERENCE) # Check to place in inner or outer lane if closest_object_pose is not None: object_point = Point([ closest_object_pose.position.x, closest_object_pose.position.y ]) object_nearest_pnts_dict = self._track_data_.get_nearest_points( object_point) object_nearest_dist_dict = self._track_data_.get_nearest_dist( object_nearest_pnts_dict, object_point) object_is_inner = object_nearest_dist_dict[TrackNearDist.NEAR_DIST_IN.value] < \ object_nearest_dist_dict[TrackNearDist.NEAR_DIST_OUT.value] if object_is_inner: start_pose = self._track_data_.outer_lane.interpolate_pose( self._track_data_.outer_lane.project( Point(start_pose.position.x, start_pose.position.y)), finite_difference=FiniteDifference.FORWARD_DIFFERENCE) else: start_pose = self._track_data_.inner_lane.interpolate_pose( self._track_data_.inner_lane.project( Point(start_pose.position.x, start_pose.position.y)), finite_difference=FiniteDifference.FORWARD_DIFFERENCE) start_model_state = ModelState() start_model_state.model_name = self._agent_name_ start_model_state.pose = start_pose start_model_state.twist.linear.x = 0 start_model_state.twist.linear.y = 0 start_model_state.twist.linear.z = 0 start_model_state.twist.angular.x = 0 start_model_state.twist.angular.y = 0 start_model_state.twist.angular.z = 0 return start_model_state
def gen_ball(self): x, y, z = self.gen_ball_loc() l_vel_x, l_vel_y, a_vel = self.gen_ball_vel((x, y)) # print("x:{} y:{} x:{}".format(x, y, z)) # print("a_vel: {} l_vel_x: {} l_vel_y: {}".format(a_vel, l_vel_x, l_vel_y)) ball_pose = self.gen_pose(x, y) ball_name = self.dodgeball_prefix + str(self.ball_num) self.ball_names.append(ball_name) # Add the ball to keep track of it ball_twist = Twist(linear=Vector3(x=l_vel_x, y=l_vel_y), angular=Vector3(z=0)) model_state = ModelState(model_name=ball_name, pose=ball_pose, twist=ball_twist) self.spawn_model(model_name=ball_name, model_xml=random.choice(self.dodgeballs), robot_namespace='', initial_pose=ball_pose) self.set_model(model_state) self.ball_num += 1
def resetobjects(self): self.land.publish(Empty()) self.land4.publish(Empty()) #del_ball_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) #del_ball_prox('volleyball') #reset world rospy.wait_for_service('/gazebo/reset_world') reset_world = rospy.ServiceProxy('/gazebo/reset_world', EM) #invoke reset_world() #reset ball rospy.wait_for_service('/gazebo/set_model_state') set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) model_state = ModelState() model_state.model_name = 'volleyball' model_state.pose = self.ball_pos model_state.reference_frame = 'world' set_model_state(model_state) #UPDATE BANNER rospy.wait_for_service('gazebo/spawn_sdf_model') spawn_baner_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) banner_left = "gradebanner" + str(self.scoreA) banner_right = "_gradebanner" + str(self.scoreB) print banner_left print banner_right fn1 = '../models/' + banner_left + '/model.sdf' fn2 = '../models/' + banner_right + '/model.sdf' ff1 = open(fn1, 'r') sdff1 = ff1.read() ff2 = open(fn2, 'r') sdff2 = ff2.read() spawn_baner_prox(banner_left, sdff1, "myworld", self.left_banner_pos, "world") spawn_baner_prox(banner_right, sdff2, "myworld", self.right_banner_pos, "world") rospy.sleep(0.5) self.takeoff.publish(Empty()) self.takeoff4.publish(Empty()) rospy.sleep(0.5)
def set_position(self, x, y, z): state_msg = ModelState() state_msg.model_name = 'three_pi' state_msg.pose.position.x = x state_msg.pose.position.y = y state_msg.pose.position.z = z state_msg.pose.orientation.x = 0 state_msg.pose.orientation.y = 0 state_msg.pose.orientation.z = 0 state_msg.pose.orientation.w = 0 rospy.wait_for_service('/gazebo/set_model_state') try: set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(state_msg) except rospy.ServiceException as e: print("Service call failed: %s" % e)
def move(time_, mvX, mvY, mvZ): ball_pose.position.x += mvX ball_pose.position.y += mvY ball_pose.position.z += mvZ state = ModelState() state.model_name = "table_tennis_ball" state.pose = ball_pose out_msg = PoseStamped() out_msg.header.frame_id = "/tt_ball" out_msg.header.stamp = rospy.Time.now() out_msg.pose = ball_pose ret = set_model_state(state) pub.publish(out_msg) print ret.status_message rospy.sleep(time_)
def set_aruco_position(self, x=0.7, y=-0.43, z=1): state_msg = ModelState() state_msg.model_name = 'aruco_cube' state_msg.pose.position.x = x state_msg.pose.position.y = y state_msg.pose.position.z = z state_msg.pose.orientation.x = 0 state_msg.pose.orientation.y = 0 state_msg.pose.orientation.z = 0 state_msg.pose.orientation.w = 1 rospy.wait_for_service('/gazebo/set_model_state') try: set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(state_msg) except rospy.ServiceException as e: print("Service call failed: %s" % e)
def movingTo(self,goal_x,goal_y): #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] == 'target': # the model name is defined in .xacro file obstacle.model_name = 'target' 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 = 0.5 self.pub_model.publish(obstacle)
def factoryModelStateBegins(modelState=ModelState(), pos=[0, 0, 0], ori=[0, 0, 0, 0], vl=[0, 0, 0], va=[0, 0, 0]): modelState.pose.position.x = pos[0] modelState.pose.position.y = pos[1] modelState.pose.position.z = pos[2] modelState.pose.orientation.x = ori[0] modelState.pose.orientation.y = ori[1] modelState.pose.orientation.z = ori[2] modelState.pose.orientation.w = ori[3] modelState.twist.linear.x = vl[0] modelState.twist.linear.y = vl[1] modelState.twist.linear.z = vl[2] modelState.twist.angular.x = va[0] modelState.twist.angular.y = va[1] modelState.twist.angular.z = va[2] return modelState
def reset_base(self): rospy.wait_for_service("/gazebo/set_model_state") set_model_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) model_state = ModelState() model_state.model_name = self.base_name self.reset_model_state(model_state) set_model_state(model_state) self.shutdown() self.terminal = False self.crashed = False self.subscriptions_ready = np.zeros(2) self.wait_for_subscriptions() self.initial_time = time.time()
def _reset(self, car_model_state): camera_model_state = ModelState() camera_model_state.model_name = self.topic_name # Calculate target Camera position based on nearest center track from the car. # 1. Project the car position to 1-d global distance of track # 2. Minus camera offset from the point of center track near_dist = self.track_data._center_line_.project( Point(car_model_state.pose.position.x, car_model_state.pose.position.y)) near_pnt_ctr = self.track_data._center_line_.interpolate(near_dist) yaw = self.track_data._center_line_.interpolate_yaw( distance=near_dist, normalized=False, reverse_dir=False, position=near_pnt_ctr) quaternion = np.array( euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=yaw)) target_camera_location = np.array([near_pnt_ctr.x, near_pnt_ctr.y, 0.0]) + \ apply_orientation(quaternion, np.array([-self.cam_dist_offset, 0, 0])) # Calculate camera rotation quaternion based on lookAt yaw with respect to # current camera position and car position look_at_yaw = utils.get_angle_between_two_points_2d_rad( Point(target_camera_location[0], target_camera_location[1]), car_model_state.pose.position) cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=look_at_yaw) camera_model_state.pose.position.x = target_camera_location[0] camera_model_state.pose.position.y = target_camera_location[1] camera_model_state.pose.position.z = self.cam_fixed_height camera_model_state.pose.orientation.x = cam_quaternion[0] camera_model_state.pose.orientation.y = cam_quaternion[1] camera_model_state.pose.orientation.z = cam_quaternion[2] camera_model_state.pose.orientation.w = cam_quaternion[3] self.model_state_client(camera_model_state) self.last_camera_state = camera_model_state self.last_yaw = yaw
def reset(self): # # Resets the state of the environment and returns an initial observation. # rospy.wait_for_service('/gazebo/reset_world') # try: # #reset_proxy.call() # self.reset_proxy() # except (rospy.ServiceException) as e: # print ("/gazebo/reset_world service call failed") sim = rospy.get_param('/use_sim_time') if sim is True: rospy.loginfo( 'Waiting until simulated robot is prepared for the task...') # rospy.wait_for_message('/sim_robot_init', EmptyMsg) rospy.loginfo("Pausing physics") self.pause_physics_client() rospy.loginfo("Reset vehicle") # define initial pose for later use pose = Pose() pose.position.x = 0.0 pose.position.y = 0.0 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 # set initial model state model_state = ModelState() model_state.model_name = "catvehicle" model_state.pose = pose self.set_model_state_client(model_state) msg = String() msg.data = "123" self.reset_sim_pub.publish(msg) rospy.loginfo("Unpausing physics") self.unpause_physics_client()
def __init__(self, hack_index=1, x=0, y=0, z=0, theta=0, radius=0.5, mass=68.0, max_speed=1.34, fov=180, agent_type=Type.person, model=Model.person_1_walking_lidar): self.hack_index = hack_index self.id = self.create_id() self.model_state = ModelState() self.agent_type = agent_type self.model = model self.name = self.model + "_" + str(self.id) self.max_speed = max_speed self.fov = fov self.radius = radius self.mass = mass self.x = x self.y = y self.z = z self.theta = theta self.force = 0.0 self.pose = Pose(position=Point(x, y, z)) self.velocity = Twist() self.last_update = None quaternion = quaternion_from_euler(0, 0, theta) self.pose.orientation.x = quaternion[0] self.pose.orientation.y = quaternion[1] self.pose.orientation.z = quaternion[2] self.pose.orientation.w = quaternion[3] self.model_state.model_name = self.name self.model_state.reference_frame = 'world' self.model_state.pose = self.pose
def __init__(self): self.initNode() self.rate = rospy.Rate(1) # TODO Why do we need this? self.correctionFact = 1. # 0.92 self.sin30 = 0.5 self.cos30 = 0.86602540378 self.runVals = np.zeros(4) self.riseVals = np.zeros(4) self.prevTwistCmd = 0 # Slope values for opposite sonars self.slopePoint = Point() self.slope02 = 0 self.slope13 = 0 self.relYaw = 0 self.sonarVals = Dvl() self.setStateMsg = ModelState() self.getStateMsg = ModelStates() self.keyboardTwist = Twist() # Subscribers # Sonar self.dvlSub = rospy.Subscriber('/whn/dvl', Dvl, self.dvlCB) # Model state self.modelStateSub = rospy.Subscriber('/gazebo/model_states', ModelStates, self.getStateCB) # Get command vel self.keyboardTwistSub = rospy.Subscriber('/cmd_vel', Twist, self.keyTwistCB) # Services self.serviceName = '/gazebo/set_model_state' self.setStateSrv = rospy.ServiceProxy(self.serviceName, SetModelState) # Publisher self.gradPointPub = rospy.Publisher('dvl_gradient', Point, queue_size=10)
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 racecar_reset(self): rospy.wait_for_service('gazebo/set_model_state') waypoints = list(self.waypoints) random_waypoint = int(np.random.choice(len(waypoints), 1)) closest_waypoint = self.get_closest_waypoint() x, y = waypoints[(random_waypoint) % len(waypoints)] x_next, y_next = waypoints[(random_waypoint + 1) % len(waypoints)] orientation = np.arctan2(y_next - y, x_next - x) modelState = ModelState() modelState.pose.position.z = 0 modelState.pose.orientation.x = 0 modelState.pose.orientation.y = 0 modelState.pose.orientation.z = orientation # 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): modelState.pose.position.x = 1.75 modelState.pose.position.y = 0.6 # modelState.pose.position.x = x # modelState.pose.position.y = y 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_pose(self): stop = Twist() time = rospy.get_time() while rospy.get_time() - time < 2: self.velcmd_pub.publish(stop) self.stop_rate.sleep() modelstate = ModelState() modelstate.model_name = self.name modelstate.reference_frame = "world" x, y, z, w = rostf.transformations.quaternion_from_euler( self.init_pose[0][3], self.init_pose[0][4], self.init_pose[0][5]) modelstate.pose.position.x = self.init_pose[0][0] modelstate.pose.position.y = self.init_pose[0][1] modelstate.pose.position.z = self.init_pose[0][2] modelstate.pose.orientation.x = x modelstate.pose.orientation.y = y modelstate.pose.orientation.z = z modelstate.pose.orientation.w = w reps = self.reset_client(modelstate)
def __init__(self): # initiliaze # self.LaunchGazebo() rospy.init_node('GazeboWorld', anonymous=False, disable_signals=True) #------------Params-------------------- self.world_start_time = time.time() self.sim_time = Clock().clock self.model_names = None self.model_poses = None #-----------Default Robot State----------------------- self.default_state = ModelState() self.default_state.model_name = None self.default_state.pose.position.x = 0. self.default_state.pose.position.y = 0. self.default_state.pose.position.z = 0. 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.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=100) self.model_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack) self.sim_clock = rospy.Subscriber('clock', Clock, self.SimClockCallBack) while self.model_names is None and not rospy.is_shutdown(): pass print "Env intialised"
def random_obstacle(self): for n in range(0, 10): state_msg = ModelState() state_msg.model_name = 'obstacle_' + str(n) #state_msg.pose.position.x = random.randint(1,9) if n < 5: state_msg.pose.position.x = n + 1 else: state_msg.pose.position.x = 4 - n state_msg.pose.position.y = random.uniform(-1, 1) #state_msg.twist.linear.x = random.uniform(-0.3,0.3) #state_msg.twist.linear.y = random.uniform(-0.3,0.3) rospy.wait_for_service('/gazebo/set_model_state') try: set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(state_msg) except (rospy.ServiceException) as e: print("Service call failed: %s" % e)
def reset_turtlebot(self): self.start_turtlebot() req = ModelState() req.model_name = "turtlebot3" req.twist.linear.x = 0.0 req.twist.linear.y = 0.0 req.twist.linear.z = 0.0 req.twist.angular.x = 0.0 req.twist.angular.y = 0.0 req.twist.angular.z = 0.0 req.pose.position.x = 0.0 req.pose.position.y = 0.0 req.pose.position.z = 0.0 req.pose.orientation.x = 0.0 req.pose.orientation.y = 0.0 req.pose.orientation.z = 0.0 req.pose.orientation.w = 0.0 self.set_state(req) self.stop_turtlebot()
def main(): rospy.init_node('reading_laser') sub = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser) model_state = ModelState() model_state.model_name = 'wheel' model_state.pose.position.x = 0 model_state.pose.position.y = 7 #model_state.pose.position.z = 0 rospy.wait_for_service('/gazebo/set_model_state') srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = srv_client_set_model_state(model_state) rospy.spin()
def __init__(self): """initialize move_group API""" rospy.init_node('gazebo_world') """init parm""" self.Robot_model = {'fetch': {'init': [0, 0, 0]}} self.Obstacle_model = {'obstacle': {'init': [0.7, 0, 0.75]}} self.Move_Obstacle_model = {'obstacle': {'init': [1.7, 0, 0.75]}} self.Fetch_init_pose_joints = [0.0, 1.5, 1.1, 0.0, -2.2, -1.8, 2, 0.4] self.camera = RGB() self.fetch = Robot() self.robot_pose = ModelState() self.pose_pub = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size=1) self.pose_get = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState) self.dis = 100
def __init__(self): # Get human's pose using service /gazebo/get_model_state self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1) rospy.wait_for_service("/gazebo/set_model_state", timeout=2) msg_model_state = ModelState() msg_model_state.model_name = "turtlebot3_burger" msg_model_state.reference_frame = "world" msg_model_state.pose.position.x = 0 msg_model_state.pose.position.y = 0 msg_model_state.pose.position.z = 0 msg_model_state.pose.orientation.x = 0 msg_model_state.pose.orientation.y = 0 msg_model_state.pose.orientation.z = 0 msg_model_state.pose.orientation.w = 1 msg_model_state.twist.linear.x = 0 msg_model_state.twist.linear.y = 0 msg_model_state.twist.linear.z = 0 msg_model_state.twist.angular.x = 0 msg_model_state.twist.angular.y = 0 msg_model_state.twist.angular.z = 0 rospy.wait_for_service("/gazebo/set_model_state", timeout=2) res_set = self.set_model_state(msg_model_state) print("set success? ", res_set) self.vel = Twist() self.vel.linear.x = 0 self.vel.linear.y = 0 self.vel.linear.z = 0 self.vel.angular.x = 0 self.vel.angular.y = 0 self.vel.angular.z = 0 for _ in range(10): self.pub.publish(self.vel) rospy.sleep(0.1) self.integral = 0 self.goal_world = np.array([0, 0])
def move_model(self, model_name, pos, rot=[0, 0, 0], reference_frame='world'): assert 'set_model_state' in self._services assert isinstance(model_name, str), 'Input model_name must be a string' assert model_name in self.get_model_names(), \ 'Model {} does not exist'.format(model_name) assert isinstance(pos, collections.Iterable) assert len(list(pos)) == 3, 'Position vector must have three ' \ 'components, (x, y, z)' for elem in pos: assert isinstance(elem, float) or isinstance(elem, int), \ '{} is not a valid number'.format(elem) assert isinstance(rot, collections.Iterable), \ 'Rotation vector must be iterable' assert len(list(rot)) == 3 or len(list(rot)) == 4 for elem in rot: assert isinstance(elem, float) or isinstance(elem, int), \ '{} is not a valid number'.format(elem) if len(list(rot)) == 3: rot = PosePCG.rpy2quat(*rot) state = ModelState() state.model_name = model_name state.pose.position.x = pos[0] state.pose.position.y = pos[1] state.pose.position.z = pos[2] state.pose.orientation.x = rot[0] state.pose.orientation.y = rot[1] state.pose.orientation.z = rot[2] state.pose.orientation.w = rot[3] state.reference_frame = reference_frame status = self._services['set_model_state'](state) return status.success
def kickBall(): state = ModelState() state.model_name = "pioneer3at" state.reference_frame = "pioneer3at" set_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) pose = Pose() twist = Twist() time.sleep(2) twist.linear.x = -3 twist.linear.y = 0 twist.angular.z = 0 state.twist = twist ret = set_state(state) time.sleep(1) twist.linear.x = 0 twist.linear.y = 0 twist.angular.z = 0 state.twist = twist ret = set_state(state)