Python msg.ModelState方法代碼示例

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def set_sailboat_heading(pub_state):
    global current_state
    global current_sail
    state_aux = ModelState()
    quaternion = (current_state.pose.pose.orientation.x, current_state.pose.pose.orientation.y, current_state.pose.pose.orientation.z, current_state.pose.pose.orientation.w)

    euler = tf.transformations.euler_from_quaternion(quaternion)

    quaternion = tf.transformations.quaternion_from_euler(euler[0], euler[1], math.radians(current_heading))
    state_aux.pose.orientation.x = quaternion[0]
    state_aux.pose.orientation.y = quaternion[1]
    state_aux.pose.orientation.z = quaternion[2]
    state_aux.pose.orientation.w = quaternion[3]
    state_aux.model_name = 'sailboat'
    #state_aux.pose.position.x = current_state.pose.pose.position.x
    #state_aux.pose.position.y = current_state.pose.pose.position.y
    #state_aux.pose.position.z = current_state.pose.pose.position.z
    #print(current_state)

    state_aux.pose.position.x = 240 
    state_aux.pose.position.y = 95
    state_aux.pose.position.z = 1
    pub_state.publish(state_aux) 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def set_model_pose(cls,
                       model_name,
                       new_pose,
                       model_reference_frame='world'):
        """
        Set the gazebo model's pose
        :param model_name: str
                    the name of new model
        :param new_pose: geometry_msgs.msg.Pose
                    the pose of new model
        :param model_reference_frame: str
                    the reference frame name(e.g. 'world')
        """
        msg = ModelState()
        msg.model_name = model_name
        msg.pose = new_pose
        msg.reference_frame = model_reference_frame
        cls.set_model_pose_pub.publish(msg) 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def set_start(self, x, y, theta):
        state = ModelState()
        state.model_name = 'robot'
        state.reference_frame = 'world'  # ''ground_plane'
        # pose
        state.pose.position.x = x
        state.pose.position.y = y
        state.pose.position.z = 0
        quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
        state.pose.orientation.x = quaternion[0]
        state.pose.orientation.y = quaternion[1]
        state.pose.orientation.z = quaternion[2]
        state.pose.orientation.w = quaternion[3]
        # twist
        state.twist.linear.x = 0
        state.twist.linear.y = 0
        state.twist.linear.z = 0
        state.twist.angular.x = 0
        state.twist.angular.y = 0
        state.twist.angular.z = 0

        rospy.wait_for_service('/gazebo/set_model_state')
        try:
            set_state = self.set_state
            result = set_state(state)
            assert result.success is True
        except rospy.ServiceException:
            print("/gazebo/get_model_state service call failed") 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def set_goal(self, x, y):
        state = ModelState()
        state.model_name = 'goal'
        state.reference_frame = 'world'  # ''ground_plane'
        state.pose.position.x = x
        state.pose.position.y = y
        state.pose.position.z = 0.1

        rospy.wait_for_service('/gazebo/set_model_state')
        try:
            set_state = self.set_state
            result = set_state(state)
            assert result.success is True
        except rospy.ServiceException:
            print("/gazebo/get_model_state service call failed") 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def set_start(x, y, theta):
    state = ModelState()
    state.model_name = 'robot'
    state.reference_frame = 'world'  # ''ground_plane'
    # pose
    state.pose.position.x = x
    state.pose.position.y = y
    state.pose.position.z = 0
    quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
    state.pose.orientation.x = quaternion[0]
    state.pose.orientation.y = quaternion[1]
    state.pose.orientation.z = quaternion[2]
    state.pose.orientation.w = quaternion[3]
    # twist
    state.twist.linear.x = 0
    state.twist.linear.y = 0
    state.twist.linear.z = 0
    state.twist.angular.x = 0
    state.twist.angular.y = 0
    state.twist.angular.z = 0

    rospy.wait_for_service('/gazebo/set_model_state')
    try:
        set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        result = set_state(state)
        assert result.success is True
        print("set the model state successfully")
    except rospy.ServiceException:
        print("/gazebo/get_model_state service call failed") 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def init_gazebo(self, st):
        rospy.set_param('gazebo/use_sim_time', True)
        try:
            ssm = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        except rospy.ServiceException as e:
            print('Service call failed: %s' % (e))
        for k in range(self._n_robots):
            pose0, twist0 = Pose(), Twist()
            pose0.position.x = st.init_pose[k].pose[0]
            pose0.position.y = st.init_pose[k].pose[1]
            x, y, z, w = tf.transformations.quaternion_from_euler(0, 0, st.init_pose[k].pose[2])
            pose0.orientation.x = x
            pose0.orientation.y = y
            pose0.orientation.z = z
            pose0.orientation.w = w
            twist0.linear.x = 0.
            twist0.angular.z = 0.
            mod0 = ModelState('p3dx'+str(k), pose0, twist0, 'world')
            ssm(mod0)
        for l, k in enumerate(st.robobst):
            pose0, twist0 = Pose(), Twist()
            pose0.position.x = st.obstacles[k].pose[0]
            pose0.position.y = st.obstacles[k].pose[1]
            x, y, z, w = tf.transformations.quaternion_from_euler(0, 0, st.obstacles[k].pose[2])
            pose0.orientation.x = x
            pose0.orientation.y = y
            pose0.orientation.z = z
            pose0.orientation.w = w
            twist0.linear.x = 0.
            twist0.angular.z = 0.
            mod0 = ModelState('p3dx_obs'+str(l), pose0, twist0, 'world')
            ssm(mod0) 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def reset(self):
        self.pause()
    
        roll, pitch, yaw = np.random.random((3,)) * np.pi
        state = LinkState()
        quaternion = tf.transformations.quaternion_from_euler(0., 0.,
            yaw)
        x, y = np.random.random((2,))
        x *= 0.2
        x += 1.1
        y *= -0.5
        y += -2

        state.pose.position.x = x
        state.pose.position.y = y
        state.pose.position.z = 0.5
        state.pose.orientation.x = quaternion[0]
        state.pose.orientation.y = quaternion[1]
        state.pose.orientation.z = quaternion[2]
        state.pose.orientation.w = quaternion[3]
        state.link_name = "base_link"
        
        #model_state = ModelState()
        #model_state.model_name = "mobile_base"
        #self.set_model_state(model_state=model_state)
        #self.set_link_state(link_state=state)
        self.index += 1
        if self.index > 10:
            self.index = 0

        self.unpause() 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def States2State(self, states, name):
		to_state = ModelState()
		from_states = copy.deepcopy(states)
		idx = from_states.name.index(name)
		to_state.model_name = name
		to_state.pose = from_states.pose[idx]
		to_state.twist = from_states.twist[idx]
		to_state.reference_frame = 'world'
		return to_state 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def init_subpub(self) :
		self.pub_modelstate = rospy.Publisher( '/gazebo/set_model_state', ModelState, queue_size=10) 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def changeSpawned(self, config=None) :
		if config is None :
			config = self.currentConfiguration

		for elem in config.shape_list :
			modelstate = ModelState()

			elpose = elem.shape_pose.tolist()[0]
			
			pose = Point()
			quat = Quaternion()

			pose.x = elpose[0]
			pose.y = elpose[1]
			pose.z = elpose[2]
			modelstate.pose.position = pose

			quaternion = tf.transformations.quaternion_from_euler(
				elpose[3],
				elpose[4],
				elpose[5]
				)
			modelstate.pose.orientation.x = quaternion[0]
			modelstate.pose.orientation.y = quaternion[1]
			modelstate.pose.orientation.z = quaternion[2]
			modelstate.pose.orientation.w = quaternion[3]

			elname = elem.getName()
			modelstate.model_name = elname


			self.pub_modelstate.publish(modelstate)

			#rospy.loginfo('CONFIGURATION SPAWNED : making changes... :\n {}'.format(modelstate) )
			#time.sleep(0.1) 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
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
        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 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
def __init__(self):
		 # initiliaze
		rospy.init_node('GazeboWorld', anonymous=False)

		#-----------Default Robot State-----------------------
		self.set_self_state = ModelState()
		self.set_self_state.model_name = 'mobile_base' 
		self.set_self_state.pose.position.x = 0.5
		self.set_self_state.pose.position.y = 0.
		self.set_self_state.pose.position.z = 0.
		self.set_self_state.pose.orientation.x = 0.0
		self.set_self_state.pose.orientation.y = 0.0
		self.set_self_state.pose.orientation.z = 0.0
		self.set_self_state.pose.orientation.w = 1.0
		self.set_self_state.twist.linear.x = 0.
		self.set_self_state.twist.linear.y = 0.
		self.set_self_state.twist.linear.z = 0.
		self.set_self_state.twist.angular.x = 0.
		self.set_self_state.twist.angular.y = 0.
		self.set_self_state.twist.angular.z = 0.
		self.set_self_state.reference_frame = 'world'

		#------------Params--------------------
		self.depth_image_size = [160, 128]
		self.rgb_image_size = [304, 228]
		self.bridge = CvBridge()

		self.object_state = [0, 0, 0, 0]
		self.object_name = []

		# 0. | left 90/s | left 45/s | right 45/s | right 90/s | acc 1/s | slow down -1/s
		self.action_table = [0.4, 0.2, np.pi/6, np.pi/12, 0., -np.pi/12, -np.pi/6]
		
		self.self_speed = [.4, 0.0]
		self.default_states = None
		
		self.start_time = time.time()
		self.max_steps = 10000

		self.depth_image = None
		self.bump = False

		#-----------Publisher and Subscriber-------------
		self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size = 10)
		self.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size = 10)
		self.resized_depth_img = rospy.Publisher('camera/depth/image_resized',Image, queue_size = 10)
		self.resized_rgb_img = rospy.Publisher('camera/rgb/image_resized',Image, queue_size = 10)

		self.object_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack)
		self.depth_image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.RGBImageCallBack)
		self.rgb_image_sub = rospy.Subscriber('camera/depth/image_raw', Image, self.DepthImageCallBack)
		self.laser_sub = rospy.Subscriber('scan', LaserScan, self.LaserScanCallBack)
		self.odom_sub = rospy.Subscriber('odom', Odometry, self.OdometryCallBack)
		self.bumper_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.BumperCallBack)

		rospy.sleep(2.)

		# What function to call when you ctrl + c    
		rospy.on_shutdown(self.shutdown) 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelState [as 別名]
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)