Python msg.ModelStates方法代碼示例

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelStates [as 別名]
def __init__(self):
        self.modelPath = os.path.dirname(os.path.realpath(__file__))
        self.modelPath = self.modelPath.replace('turtlebot3_machine_learning/turtlebot3_dqn/src/turtlebot3_dqn',
                                                'turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_square/goal_box/model.sdf')
        self.f = open(self.modelPath, 'r')
        self.model = self.f.read()
        self.stage = rospy.get_param('/stage_number')
        self.goal_position = Pose()
        self.init_goal_x = 0.6
        self.init_goal_y = 0.0
        self.goal_position.position.x = self.init_goal_x
        self.goal_position.position.y = self.init_goal_y
        self.modelName = 'goal'
        self.obstacle_1 = 0.6, 0.6
        self.obstacle_2 = 0.6, -0.6
        self.obstacle_3 = -0.6, 0.6
        self.obstacle_4 = -0.6, -0.6
        self.last_goal_x = self.init_goal_x
        self.last_goal_y = self.init_goal_y
        self.last_index = 0
        self.sub_model = rospy.Subscriber('gazebo/model_states', ModelStates, self.checkModel)
        self.check_model = False
        self.index = 0 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelStates [as 別名]
def __init__(self, sample_time, update_time, n_robots, obst_traj=[]):
        rospy.init_node('p3dx_controller')
        self._sample_time = sample_time
        self._update_time = update_time
        self._mp_status = False
        self._n_robots = n_robots
        self._obst_traj = obst_traj
        self._robobst = list(obst_traj.keys())
        self._robot_est_pose = [[0., 0., 0.] for k in range(n_robots)]
        self._robot_real_pose = [[0., 0., 0.] for k in range(n_robots)]
        self._robobst_est_pose = [[0., 0.] for k in range(len(self._robobst))]
        self._robobst_est_velocity = [[0., 0.] for k in range(len(self._robobst))]
        self._vel_traj = [{'v': [], 'w': []} for k in range(n_robots)]
        self._vel_traj_applied = [{'v': [], 'w': []} for k in range(n_robots)]
        self._cmd_vel_topic = [rospy.Publisher('robot'+str(k)+'/p3dx/cmd_vel', Twist, queue_size=1) for k in range(n_robots)]
        self._cmd_vel_robobst_topic = [rospy.Publisher('robobst'+str(l)+'/p3dx/cmd_vel', Twist, queue_size=1) for l in range(len(self._robobst))]

        self._mp_trigger_topic = rospy.Publisher('mp_trigger', Trigger, queue_size=1)
        self._mp_configure_topic = rospy.Publisher('mp_configure', Settings, queue_size=1)
        rospy.Subscriber('mp_result', FleetTrajectories, self.get_mp_result)
        rospy.Subscriber('mp_feedback', Bool, self.get_mp_feedback)
        # for k in range(n_robots):
        #     rospy.Subscriber('robot'+str(k)+'/p3dx/base_pose_ground_truth', Odometry, callback=self.get_est_pose, callback_args=k)
        rospy.Subscriber('/gazebo/model_states', ModelStates, callback=self.get_model_states)
        # for l in range(len(self._robobst)):
        #     rospy.Subscriber('robobst'+str(l)+'/p3dx/base_pose_ground_truth', Odometry, callback=self.get_est_pose_robobst, callback_args=l) 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelStates [as 別名]
def listener():
    # setup subscribers
    rospy.init_node('costar_simulator_husky', anonymous=True)
    
    #rospy.Subscriber(gazeboModelsTopic, ModelStates, modelsCallback)
    rospy.Subscriber(overheadImageTopic, Image, imageCallback)

    # object names and poses

    # overhead camera 

# 需要導入模塊: from gazebo_msgs import msg [as 別名]
# 或者: from gazebo_msgs.msg import ModelStates [as 別名]
def initialize(self):
        if self._simulated:
            Gazebo.load_gazebo_model(
                'table',
                Pose(position=Point(x=0.75, y=0.0, z=0.0)),
                osp.join(World.MODEL_DIR, 'cafe_table/model.sdf'))
            Gazebo.load_gazebo_model(
                'block',
                Pose(position=Point(x=0.5725, y=0.1265, z=0.90)),
                osp.join(World.MODEL_DIR, 'block/model.urdf'))
            block = Block(
                name='block',
                initial_pos=(0.5725, 0.1265, 0.90),
                random_delta_range=0.15,
                resource=osp.join(World.MODEL_DIR, 'block/model.urdf'))
            # Waiting models to be loaded
            rospy.sleep(1)
            self._block_states_subs.append(
                rospy.Subscriber('/gazebo/model_states', ModelStates,
                                 self._gazebo_update_block_states))
            self._blocks.append(block)
        else:
            for vicon_topic in VICON_TOPICS:
                block = Block(
                    name='block',
                    initial_pos=(0.5725, 0.1265, 0.90),
                    random_delta_range=0.15,
                    resource=vicon_topic)
                self._block_states_subs.append(
                    rospy.Subscriber(block.resource, TransformStamped,
                                     self._vicon_update_block_states))
                self._blocks.append(block)

        # Add table to moveit
        pose_stamped = PoseStamped()
        pose_stamped.header.frame_id = self._frame_id
        pose_stamped.pose.position.x = 0.655
        pose_stamped.pose.position.y = 0
        # Leave redundant space
        pose_stamped.pose.position.z = -0.02
        pose_stamped.pose.orientation.x = 0
        pose_stamped.pose.orientation.y = 0
        pose_stamped.pose.orientation.z = 0
        pose_stamped.pose.orientation.w = 1.0
        self._moveit_scene.add_box('table', pose_stamped, (1.0, 0.9, 0.1)) 

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