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 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 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 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 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 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 set_state(self, x, y, yaw): set_quaternion = tf.transformations.quaternion_from_euler(0., 0., yaw) next_state = ModelState() next_state.model_name = 'pmb2' next_state.pose.position.x = x next_state.pose.position.y = y next_state.pose.position.z = 0.001 next_state.pose.orientation.x = set_quaternion[0] next_state.pose.orientation.y = set_quaternion[1] next_state.pose.orientation.z = set_quaternion[2] next_state.pose.orientation.w = set_quaternion[3] resp_set = self.set_coordinates(next_state)
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(): #hello_str = "hello world %s" % rospy.get_time() 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 = temp[i][2] #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) rot_q = x.pose.orientation (roll, pitch, theta) = euler_from_quaternion( [rot_q.x, rot_q.y, rot_q.z, rot_q.w]) if (i < len(temp) - 1): angle_to_goal = atan2(temp[i + 1][1] - temp[i][1], temp[i + 1][0] - temp[i + 1][0]) if abs(angle_to_goal - theta) > 0.1: x.twist.linear.x = 0.0 x.twist.angular.z = 0.0 else: x.twist.linear.x = 0.5 x.twist.angular.z = 0.0 #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 odomCallback (odomMsg): state_msg = ModelState() state_msg.model_name = 'turtlebot3_waffle' state_msg.pose = odomMsg.pose.pose #state_msg.twist = odomMsg.twist.twist 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, e): print ("Service call failed: %s" % e)
def _reset_obstacles(self): for obstacle_name, obstacle_pose in zip(self.obstacle_names, self.obstacle_poses): obstacle_state = ModelState() obstacle_state.model_name = obstacle_name obstacle_state.pose = obstacle_pose obstacle_state.twist.linear.x = 0 obstacle_state.twist.linear.y = 0 obstacle_state.twist.linear.z = 0 obstacle_state.twist.angular.x = 0 obstacle_state.twist.angular.y = 0 obstacle_state.twist.angular.z = 0 SetModelStateTracker.get_instance().set_model_state(obstacle_state)
def main(): # Initiate node for allocating a model at a certain position rospy.init_node('set_model_position', anonymous=True) # Setup Services for pausing & resuming gazebo physics # and change model states mypause = rospy.ServiceProxy('/gazebo/pause_physics', Empty) myunpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) set_pos = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # Set pose of the model pose = Pose() pose.position.x = 0.0 pose.position.y = 2.0 pose.position.z = 1.65 pose.orientation.x = 0.0 pose.orientation.y = 0.0 pose.orientation.z = 0.0 pose.orientation.w = 1.0 # Set robot ought to be moved state = ModelState() state.model_name = "ball" state.pose = pose state.twist.linear.x = 0.0 state.twist.linear.y = 0.0 state.twist.linear.z = 0.0 state.twist.angular.x = 0.0 state.twist.angular.y = 0.0 state.twist.angular.z = 0.0 state.reference_frame = 'world' rate = rospy.Rate(2) # 25hz i = 0 while not rospy.is_shutdown(): #Pause physics try: mypause() except Exception, e: rospy.logerr('Error on Calling Service: %s', str(e)) # Renew the position of the model pose.position.z = 1.65 + math.sin(i / 3.) pose.position.x = math.cos(i / 3.) state.pose = pose # Call the Service to publish position info #rospy.wait_for_service('/gazebo/set_model_state') try: return_msg = set_pos(state) # print return_msg.status_message except Exception, e: rospy.logerr('Error on Calling Service: %s', str(e))
def get_initial_state(self, id): # start position state_msg = ModelState() state_msg.model_name = 'X1' state_msg.pose.position.x = INITIAL_STATES[id][0] state_msg.pose.position.y = INITIAL_STATES[id][1] state_msg.pose.position.z = INITIAL_STATES[id][2] state_msg.pose.orientation.x = INITIAL_STATES[id][3] state_msg.pose.orientation.y = INITIAL_STATES[id][4] state_msg.pose.orientation.z = INITIAL_STATES[id][5] state_msg.pose.orientation.w = INITIAL_STATES[id][6] return state_msg
def set_object(self, name, pos, ori): msg = ModelState() msg.model_name = name msg.pose.position.x = pos[0] msg.pose.position.y = pos[1] msg.pose.position.z = pos[2] msg.pose.orientation.w = ori[0] msg.pose.orientation.x = ori[1] msg.pose.orientation.y = ori[2] msg.pose.orientation.z = ori[3] msg.reference_frame = 'world' self.set_mode_pub.publish(msg)
def create_model_state(x, y, z, angle): model_state = ModelState() model_state.model_name = 'jackal' model_state.pose.position.x = x model_state.pose.position.y = y model_state.pose.position.z = z e = qt(axis=[0, 0, 1], angle=angle).elements model_state.pose.orientation = Quaternion(e[1], e[2], e[3], e[0]) model_state.reference_frame = "world" return model_state
def main(): global regions_, position_, desired_position_, state_, yaw_,pitch_,roll_,throttle_, yaw_error_allowed_, pitch_error_allowed_, roll_error_allowed_, throttle_error_allowed_ global srv_client_go_to_point_, srv_client_wall_follower_ global count_state_time_, count_loop_ rospy.init_node('bug0') sub_laser = rospy.Subscriber('/m2wr/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 = 'm2wr' model_state.pose.position.x = initial_position_.x model_state.pose.position.y = initial_position_.y model_state.pose.position.z = initial_position_.z 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 distance_position_to_line = distance_to_line(position_) if state_ == 0: if regions_['front'] > 0.15 and regions_['front'] < 1: change_state(1) elif state_ == 1: if count_state_time_ > 5 and \ distance_position_to_line < 0.1: change_state(0) count_loop_ = count_loop_ + 1 if count_loop_ == 20: count_state_time_ = count_state_time_ + 1 count_loop_ = 0 rospy.loginfo("distance to line: [%.2f], position: [%.2f, %.2f]", distance_to_line(position_), position_.x, position_.y, position_.z) rate.sleep()
def main(): rospy.init_node('set_pose') steps_params = rospy.get_param("~steps", "0, 0, 0.005") steps = [float(x.strip()) for x in steps_params.split(",")] print(steps) steps[0] = rospy.get_param("~x_step", steps[0]) steps[1] = rospy.get_param("~y_step", steps[1]) steps[2] = rospy.get_param("~z_step", steps[2]) max_pos_params = rospy.get_param("~max_pos", "1, 1, 5") max_pos = [float(x.strip()) for x in max_pos_params.split(",")] min_pos_params = rospy.get_param("~min_pos", "-1, -1, 0") min_pos = [float(x.strip()) for x in min_pos_params.split(",")] model_name = rospy.get_param("~model_name", "wall") init_pos_params = rospy.get_param("~init_pos", "0, 0, 2") pos = [float(x.strip()) for x in init_pos_params.split(",")] print(steps) print(max_pos) print(min_pos) print(model_name) rospy.wait_for_service('/gazebo/set_model_state') inc = steps[:] #copy the array? not sure if not having [:] copies by pointer. while not rospy.is_shutdown(): state_msg = ModelState() state_msg.model_name = model_name state_msg.pose.position.x = pos[0] state_msg.pose.position.y = pos[1] state_msg.pose.position.z = pos[2] state_msg.pose.orientation.x = 0 state_msg.pose.orientation.y = 0 state_msg.pose.orientation.z = 0 state_msg.pose.orientation.w = 0 for i in range(3): if pos[i] >= max_pos[i]: inc[i] = -1 * steps[i] if pos[i] <= min_pos[i]: inc[i] = steps[i] pos[i] += inc[i] #print(pos) 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) rospy.sleep(0.001)
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 directMove(self): # initiliaze # note: the name tag in the Dmstar launch file will overwrite the name of this # take the next waypoint, delete previous waypoint and publish the path if not self.moveFlag: return if len(self.path.poses) == 1: self.path.poses.append(self.path.poses[0]) current_pose = self.path.poses[0] next_pose = self.path.poses[1] # current_pose = self.path.poses[self.counter] # next_pose = self.path.poses[self.counter + 1] # print("agent %d" % self.selfID) # print(next_pose) if (len(self.path.poses) >= 2): del (self.path.poses[0]) self.selfPathPub.publish(self.path) # loop till the next point is reached yaw_diff = next_pose.yaw - current_pose.yaw # wrap up to 0 - pi/2 if yaw_diff >= 2: yaw_diff -= 4 elif yaw_diff <= -2: yaw_diff += 4 step = ((next_pose.x - current_pose.x) / self.frequency, (next_pose.y - current_pose.y) / self.frequency, (yaw_diff) / self.frequency) # start looping for i in range(self.frequency): pose = Pose() pose.position.x = 1.2 * (current_pose.x + i * step[0]) + 0.6 pose.position.y = -(1.2 * (current_pose.y + i * step[1]) + 0.6) pose.position.z = 0.1 pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = math.sin( -(current_pose.yaw + i * step[2] + 2) / 2 * math.pi / 2) pose.orientation.w = math.cos( -(current_pose.yaw + i * step[2] + 2) / 2 * math.pi / 2) state = ModelState() state.model_name = 'Husky_' + 'agent' + str(self.selfID) state.pose = pose self.modelStatePub.publish(state) self.rate.sleep() self.counter += 1
def main(): set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration) for i in range(10000): model_msg = ModelState() model_msg.model_name = "humanoid" model_msg.reference_frame = "world" model_msg.pose.position.x = 0.6 model_msg.pose.position.y = 0.0 model_msg.pose.position.z = -0.4 resp_set = set_model_state(model_msg)
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 set_target(self): for i, pos in enumerate(self.target_pos): rospy.wait_for_service('gazebo/set_model_state') try: state = ModelState() state.model_name = self.name_target + str(i) state.reference_frame = "world" state.pose.position.x = pos[0] state.pose.position.y = pos[1] self.set_model(state) except rospy.ServiceException, e: print("/gazebo/set_model_state service call failed")
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 teleport(self, location_pose, agent='movo'): """ places movo in the desired location Parameters: ----------- location_pose: Pose data type for desired location of movo """ teleport_loc = ModelState() teleport_loc.model_name = agent teleport_loc.pose = location_pose self.set_model_state(teleport_loc) rospy.sleep(1)
def random_start(self): state_msg = ModelState() state_msg.model_name = 'robot' state_msg.pose.position.x = random.randint(0, 9) state_msg.pose.position.y = random.uniform(-1, 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 setModelState(self, currState, targetState): #control = AckermannDrive() control = self.rearWheelFeedback(currState, targetState) values = self.rearWheelModel(control) newState = ModelState() newState.model_name = 'polaris_ranger_ev' newState.pose = currState.pose newState.twist.linear.x = 0 #values[0] newState.twist.linear.y = .5 #values[1] newState.twist.angular.z = values[2] self.modelStatePub.publish(newState)
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 set_model_state_client(name, position, orientation): state_msg = ModelState() state_msg.model_name = name state_msg.pose.position = position state_msg.pose.orientation = orientation rospy.wait_for_service('/gazebo/set_model_state') try: set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) response = set_model_state(state_msg) return response except rospy.ServiceException as e: print("Service call failed: %s", e)
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 reset(self): # Reset Turtlebot 1 position state_msg = ModelState() state_msg.model_name = 'tb3_0' state_msg.pose.position.x = -1.0 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 # Reset Turtlebot 2 Position state_msg2 = ModelState() state_msg2.model_name = 'tb3_1' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0' state_msg2.pose.position.x = 1.0 state_msg2.pose.position.y = 0.0 state_msg2.pose.position.z = 0.0 state_msg2.pose.orientation.x = 0 state_msg2.pose.orientation.y = 0 state_msg2.pose.orientation.z = -0.2 state_msg2.pose.orientation.w = 0 rospy.wait_for_service('gazebo/reset_simulation') # Request service to reset the position of robots rospy.wait_for_service('/gazebo/set_model_state') try: set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(state_msg) resp = set_state(state_msg2) #resp_targ = set_state(state_target_msg) except rospy.ServiceException as e: print("Service Call Failed: %s"%e) for i in range(self.num_robots): self.move_cmd[i].linear.x = 0.0 self.move_cmd[i].angular.z = 0.0 self.pub_tb3[i].publish(self.move_cmd[i])
def get_initial_state(self, name, id): # start position state_msg = ModelState() state_msg.model_name = name state_msg.pose.position.x = INITIAL_STATES[id][0] state_msg.pose.position.y = INITIAL_STATES[id][1] state_msg.pose.position.z = INITIAL_STATES[id][2] quat = quaternion_from_euler(0, 0, np.random.uniform(-np.pi, np.pi)) 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] return state_msg
def reset_ball(self): while self.BallPosX != -680: Ball_msg = ModelState() Ball_msg.model_name = 'football' Ball_msg.pose.position.x = -6.8 #-6 #-6.8 Ball_msg.pose.position.y = random.uniform(-3.3, 3.3) # Ball_msg.pose.position.y = 0 Ball_msg.pose.position.z = 0.12 Ball_msg.pose.orientation.x = 0 Ball_msg.pose.orientation.y = 0 Ball_msg.pose.orientation.z = 0 Ball_msg.pose.orientation.w = 1 self.reset_pub.publish(Ball_msg)
def reset_A(self): while self.NunbotAPosX != -830: A_msg = ModelState() A_msg.model_name = 'nubot1' A_msg.pose.position.x = -8.3 #-8 #-8.5 A_msg.pose.position.y = random.uniform(-1.7, 1.7) #0 # A_msg.pose.position.y = 0 A_msg.pose.position.z = 0 A_msg.pose.orientation.x = 0 A_msg.pose.orientation.y = 0 A_msg.pose.orientation.z = 0 A_msg.pose.orientation.w = 1 self.reset_pub.publish(A_msg)
def set_velocity(self, model_name, vx, vy, vz): rospy.wait_for_service("/gazebo/get_model_state") state = self.get_model_state(model_name, "world") print("velocity:\t" + str(state.pose.position.x) + "\t\t" + str(state.pose.position.y)) msg_model_state = ModelState() msg_model_state.model_name = model_name msg_model_state.reference_frame = "world" msg_model_state.pose = state.pose msg_model_state.twist.linear.x = vx msg_model_state.twist.linear.y = vy msg_model_state.twist.linear.z = vz self.set_model_state(msg_model_state)
def _pause_car_model(self): '''Pause agent immediately at the current position ''' car_model_state = ModelState() car_model_state.model_name = self._agent_name_ car_model_state.pose = self.car_model_state.pose car_model_state.twist.linear.x = 0 car_model_state.twist.linear.y = 0 car_model_state.twist.linear.z = 0 car_model_state.twist.angular.x = 0 car_model_state.twist.angular.y = 0 car_model_state.twist.angular.z = 0 self.set_model_state(car_model_state)
def moveInGazebo(x, y, color): #calls gazebo service to place red pieces #receives position to be placed state_msg = ModelState() if color=="red": state_msg.model_name = red_model_name.pop(0) elif color=="blue": state_msg.model_name = blue_model_name.pop(0) state_msg.reference_frame = 'world' state_msg.pose.position.x = x state_msg.pose.position.y = y state_msg.pose.position.z = 0.20 q = quaternion_from_euler(0, -pi, 0) state_msg.pose.orientation = Quaternion(x = q[0], y = q[1], z = q[2], w = q[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, e: print "Service call failed: %s" % e
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 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 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.")
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': break
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) def reset_cylinder():
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))
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) x_values_flip = np.arange(5, -5, -0.0009, dtype=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()
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