在本教程的前几部分（第 1 部分、第 2 部分和第 3 部分）中，我们已经安装并训练了我们的神经网络来移动移动机器人平台。我们观察环境，然后采取行动，最终将机器人引向我们的目标。但直到现在，环境一直是相当抽象的。我们指定在 ROS 凉亭模拟器中执行操作，但这种交互实际上是如何发生的？我们如何控制它？为了找到这个问题的答案，我们将研究以下环境代码
velodyne_env.py类初始化
如前所述，我们将使用 ROS Gazebo 模拟器作为骨干，与机器人一起执行操作并收集环境观测值。为此，我们将创建一个 python 类，以促进神经网络代码和模拟器之间的这种交换。
class GazeboEnv:
"""Superclass for all Gazebo environments."""首先，我们将初始化我们的类。这将使用描述的设置设置并自动启动我们的模拟器。
def __init__(self, launchfile, environment_dim):
self.environment_dim = environment_dim
self.odom_x = 0
self.odom_y = 0
self.goal_x = 1
self.goal_y = 0.0
self.upper = 5.0
self.lower = -5.0
self.velodyne_data = np.ones(self.environment_dim) * 10
self.last_odom = None
self.set_self_state = ModelState()
self.set_self_state.model_name = "r1"
self.set_self_state.pose.position.x = 0.0
self.set_self_state.pose.position.y = 0.0
self.set_self_state.pose.position.z = 0.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作为输入参数，初始化函数采用“launchfile”的名称。启动文件是一个 ROS 文件，用于描述模拟的启动参数，位于：
TD3/assets在第 3 部分的示例中，我们传入了“multi_robot_scenario.launch”文件。我们传入的另一个参数是我们在训练文件中指定的environment_dim值。回想一下，这是我们想要描述周围环境的激光读数的数量（在我们的例子中，是 20 个读数）。在初始化过程中，我们设置了初始类变量。我们在 X 和 Y 坐标中设置起始里程计信息。然后，我们还设置了目标 X 和 Y 坐标的占位符。self.upper 和 self.lower 变量指定随机目标位置与机器人相距多远的最大值和最小值（我们稍后会谈到这一点）。之后，我们创建self.velodyne_state变量来存储我们的状态观测值。最初，我们用值 10 填充它。我们选择 10，因为这将是我们为激光读数指定的最大距离（以米为单位）。为上次收到的里程计数据创建占位符。然后，创建占位符以指定机器人的设置位置（我们将在特定位置重置机器人时调用这些值）。
在下一部分中，我们将创建用于激光数据分箱的间隙。
self.gaps = [[-np.pi / 2 - 0.03, -np.pi / 2 + np.pi / self.environment_dim]]
for m in range(self.environment_dim - 1):
self.gaps.append(
[self.gaps[m][1], self.gaps[m][1] + np.pi / self.environment_dim]
)
self.gaps[-1][-1] += 0.03考虑一个视野为 180 度的激光读数（就像我们的情况一样）。每个激光值都是以特定角度记录的，我们可以有很多这样的记录。假设我们在 180 度视野中记录了 180 个值。但是我们指定要只用 20 个值来表示环境（因为网络的训练速度更快，输入参数更少）。这意味着我们需要以某种方式将 180 个值的数量减少到 20 个。我们通过将 180 度视场分成 20 个间隙来记录每个间隙的最小激光值。因此，在初始化过程中，我们计算每个间隙的起始和结束角度，并保存以备后用。
现在我们已经设置了占位符，我们可以开始初始化和启动模拟器了。
port = "11311"
subprocess.Popen(["roscore", "-p", port])
print("Roscore launched!")我们通过在初始化函数中调用此过程，在端口 11311 启动 roscore。
然后使用输入启动文件启动模拟器。
# Launch the simulation with the given launchfile name
rospy.init_node("gym", anonymous=True)
if launchfile.startswith("/"):
fullpath = launchfile
else:
fullpath = os.path.join(os.path.dirname(__file__), "assets", launchfile)
if not path.exists(fullpath):
raise IOError("File " + fullpath + " does not exist")
#如果launchfile以斜杠"/"开头，则将其视为绝对路径；否则，将其视为相对路径，
#然后与当前文件(file)所在目录、"assets"目录和launchfile拼接得到完整路径fullpath。
subprocess.Popen(["roslaunch", "-p", port, fullpath])
print("Gazebo launched!")现在我们可以设置我们的 ROS 发布者和订阅者，这将允许我们从模拟器发送和接收信息。
# Set up the ROS publishers and subscribers
self.vel_pub = rospy.Publisher("/r1/cmd_vel", Twist, queue_size=1)
self.set_state = rospy.Publisher(
"gazebo/set_model_state", ModelState, queue_size=10
)
self.unpause = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
self.pause = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
self.reset_proxy = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.publisher = rospy.Publisher("goal_point", MarkerArray, queue_size=3)
self.publisher2 = rospy.Publisher("linear_velocity", MarkerArray, queue_size=1)
self.publisher3 = rospy.Publisher("angular_velocity", MarkerArray, queue_size=1)
self.velodyne = rospy.Subscriber(
"/velodyne_points", PointCloud2, self.velodyne_callback, queue_size=1
)
self.odom = rospy.Subscriber(
"/r1/odom", Odometry, self.odom_callback, queue_size=1
)self.vel_pub发布者将通过 Twist（twist 是 ROS 消息类型）消息以线性和角速度的形式将动作信息发送到模拟器中的机器人。self.set_state将发布机器人并将其放置在模拟器中的指定位置。我们将使用它在新剧集的每个开始时随机放置机器人。self.unpause 和 self.pause 函数将暂停和取消暂停模拟器。self.reset_proxy重置整个模拟。发布者 self.publisher / self.publisher2 / self.publisher3 将在 Rviz 可视化模拟中直观地发布有关目标位置、当前线性和当前角速度的信息。我们将使用 self.velodyne 订阅器从 velodyne LiDAR 接收激光数据（注意，它接收点云数据而不是激光距离）。最后，我们使用 self.odom subscriber 获取模拟器中的机器人位置。
这里需要注意的是，self.velodyne 和 self.odom 订阅者分别调用回调函数 self.velodyne_callback 和 self.odom_callback。简单地说，回调是一个函数，每次从这个订阅者那里获得新数据时都会调用它。通常，这样做是为了以某种方式处理和准备数据。
def odom_callback(self, od_data):
self.last_odom = od_dataodom_callback只是将上次收到的里程计数据存储在 last_odom 变量中。
velodyne_callback更复杂一些。
# Read velodyne pointcloud and turn it into distance data, then select the minimum value for each angle
# range as state representation
def velodyne_callback(self, v):
data = list(pc2.read_points(v, skip_nans=False, field_names=("x", "y", "z")))
self.velodyne_data = np.ones(self.environment_dim) * 10
for i in range(len(data)):
if data[i][2] > -0.2:
#计算点云数据在x轴上的投影（dot），即点乘操作
dot = data[i][0] * 1 + data[i][1] * 0
#计算点云数据在x轴和y轴上的向量长度（mag1和mag2）
mag1 = math.sqrt(math.pow(data[i][0], 2) + math.pow(data[i][1], 2))
mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
#计算点云数据在x轴和y轴之间的夹角（beta），使用反余弦函数计算。
beta = math.acos(dot / (mag1 * mag2)) * np.sign(data[i][1])
dist = math.sqrt(data[i][0] ** 2 + data[i][1] ** 2 + data[i][2] ** 2)
for j in range(len(self.gaps)):
if self.gaps[j][0] <= beta < self.gaps[j][1]:
self.velodyne_data[j] = min(self.velodyne_data[j], dist)
break我们指出，velodyne用户以点云的形式返回数据。这意味着每个记录的点都会返回每个点的每个激光读数的 X、Y、Z 坐标。但是，我们希望激光读数像 2D LiDAR 一样表示，其中每个激光值返回到障碍物的距离。这意味着我们需要处理然后“展平”velodyne数据，然后才能使用它。首先，我们从传入数据中读取 X、Y 和 Z 值。然后创建一个占位符，用于存储处理后的 velodyne 数据的位置。此数据现在将设置为 10，表示最大测量距离。（注意：如果激光指向空白空间，则不会返回测量值，并且数据点将设置为 NaN。但是，神经网络无法处理 NaN，因此在这种情况下，我们需要将这些值替换为最大测量范围。读取数据后，我们就可以开始过滤每个传入的数据点。首先，我们通过检查数据点的 z 坐标是否大于 -0.2（数据的原点是 velodyne 传感器）来过滤掉地面。然后我们计算到数据点的距离。一旦我们有了距离，我们就会计算出这个测量值属于哪个间隙。如果到数据点的距离小于此间隙的现有距离，则将此距离保存为此间隙的代表性度量。这使我们能够使用 3D velodyne LiDAR 传感器来记录多个高度的障碍物，但仍然以 1D 矢量的形式获得激光距离表示。简单地说，我们只是保留每个间隙的最小值，从而“展平”3D激光数据。
通过16通道3D Velodyne LiDAR进行环境表示
一步step
初始化类后，我们可以指定将在模拟器中执行单个运动步骤的函数。
def step(self, action):
target = False
# Publish the robot action
vel_cmd = Twist()
vel_cmd.linear.x = action[0]
vel_cmd.angular.z = action[1]
self.vel_pub.publish(vel_cmd)
#调用self.publish_markers(action)函数，发布与动作相关的可视化标记。
self.publish_markers(action)
rospy.wait_for_service("/gazebo/unpause_physics")
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
# propagate state for TIME_DELTA seconds
time.sleep(TIME_DELTA)
rospy.wait_for_service("/gazebo/pause_physics")
try:
pass
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")对于一个步骤，我们传递一个操作来执行此操作并将其作为 Twist（） 消息发布给我们的机器人。我们还通过发布标记来可视化动作和目标位置。由于我们不希望在后台执行某些计算时运行模拟，因此当我们不执行操作时，我们会暂停它。这意味着要使用当前动作传播状态，我们必须取消暂停模拟。然后传播它TIME_DELTA一段时间（在此实现中为 0.1 秒）并再次暂停。这一系列动作将执行该动作 0.1 秒，并允许我们读取它之后的新状态。
现在我们阅读状态
# read velodyne laser state
done, collision, min_laser = self.observe_collision(self.velodyne_data)
v_state = []
v_state[:] = self.velodyne_data[:]
laser_state = [v_state]
# Calculate robot heading from odometry data
#从里程计数据中计算机器人的航向角。
#首先，从self.last_odom中获取机器人的位置坐标odom_x和odom_y。然后，从
#self.last_odom.pose.pose.orientation中获取四元数表示的姿态信息，
#并将其转换为欧拉角表示的姿态信息。最后，从欧拉角中提取机器人的航向角，
#并将其四舍五入到四位小数。
self.odom_x = self.last_odom.pose.pose.position.x
self.odom_y = self.last_odom.pose.pose.position.y
quaternion = Quaternion(
self.last_odom.pose.pose.orientation.w,
self.last_odom.pose.pose.orientation.x,
self.last_odom.pose.pose.orientation.y,
self.last_odom.pose.pose.orientation.z,
)
euler = quaternion.to_euler(degrees=False)
angle = round(euler[2], 4)我们的回调函数会以高频率不断更新后台的velodyne_data，因此我们将在传播状态后立即收到最新的激光观测结果。有了这些信息，我们可以计算，如果发生了冲突，并做一些快速的数据操作来保存laser_state（我们复制它以免改变velodyne数据，并准备正确的列表形式）。从里程计订阅者那里，我们获得机器人的位置信息。机器人的航向以四元数的形式返回。为了把它变成人类可以真正理解的东西，我们把它翻译成一个欧拉角，并在小数点后四舍五入到4位数字。
现在我们知道了机器人的位置和方向，我们可以计算目标的极坐标。
# Calculate distance to the goal from the robot
distance = np.linalg.norm(
[self.odom_x - self.goal_x, self.odom_y - self.goal_y]
)
# Calculate the relative angle between the robots heading and heading toward the goal
skew_x = self.goal_x - self.odom_x
skew_y = self.goal_y - self.odom_y
dot = skew_x * 1 + skew_y * 0
mag1 = math.sqrt(math.pow(skew_x, 2) + math.pow(skew_y, 2))
mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
beta = math.acos(dot / (mag1 * mag2))
if skew_y < 0:
if skew_x < 0:
beta = -beta
else:
beta = 0 - beta
theta = beta - angle
if theta > np.pi:
theta = np.pi - theta
theta = -np.pi - theta
if theta < -np.pi:
theta = -np.pi - theta
theta = np.pi - theta对于到目标的距离，我们使用 numpy.linalg.norm（）函数，该函数将返回给定向量的长度。如果我们给出目标的坐标，相对于机器人的位置，这将为我们提供距离信息。然后，我们实现一种笨拙的方法来计算朝向目标的向量和机器人航向之间的航向差异（请随时更新这部分代码，并回复我更好的解决方案）。从本质上讲，我们需要对齐坐标系并获得角度的差异。这为我们提供了距离和 theta 来表示机器人框架中目标的极坐标。
通过获得的距离信息，我们可以看到我们是否达到了目标。
# Detect if the goal has been reached and give a large positive reward
if distance < GOAL_REACHED_DIST:
target = True
done = True如果距离小于GOAL_REACHED_DIST阈值（这里是 0.3 米），我们就达到了目标。
最后，我们可以组合完整的状态信息，并将其与其他相关信息一起返回。
robot_state = [distance, theta, action[0], action[1]]
state = np.append(laser_state, robot_state)
reward = self.get_reward(target, collision, action, min_laser)
return state, reward, done, target我们将机器人状态与激光状态相结合。然后获得奖励。最后，我们返回状态、奖励、剧集是否因某种原因结束，以及我们是否达到了目标。
注意：您会注意到，我们在这里使用了几个类函数。首先，我们有一个功能，看看我们是否与任何东西相撞。
@staticmethod
def observe_collision(laser_data):
# Detect a collision from laser data
min_laser = min(laser_data)
if min_laser < COLLISION_DIST:
return True, True, min_laser
return False, False, min_laser我们检查激光数据并选择最小值。当然，如果最小值小于COLLISION_DIST阈值，则返回已发生碰撞。我们还返回将用于奖励计算的最小激光值。
奖励
@staticmethod
def get_reward(target, collision, action, min_laser):
if target:
return 100.0
elif collision:
return -100.0
else:
r3 = lambda x: 1 - x if x < 1 else 0.0
return action[0] / 2 - abs(action[1]) / 2 - r3(min_laser) / 2如果机器人到达目标，我们会给予高奖励，如果机器人与任何东西相撞，我们会给予负面奖励，否则会立即给予奖励。即时奖励的基本形式可以表示为：
r = v - |ω|它背后的想法是，机器人需要意识到它应该四处移动，而不仅仅是坐在一个地方。通过为线性运动设置正奖励，机器人首先了解到向前移动是好的，而旋转则不是。尽管一开始崩溃了很多，但剧集奖励仍然比坐在一个地方旋转要高。很快它就知道，在障碍物附近转弯比撞车更有利，尽管它仍然可能是一个消极的动作，但它比撞车更不利。有了这个，机器人很快就学会了以平稳的运动避开障碍物，因为旋转越小，转弯的惩罚就越小。在环境中奔跑时，机器人最终会随机地达到目标，并及时意识到达到目标的好处超过了转弯的惩罚。即使它最终落入口袋，机器人仍然会知道向前运动会给它带来积极的奖励，因此会无意中寻找出路。但是，这种方法并不能为您提供实现目标的最短路径，因为机器人在进行旋转运动时也更喜欢线性运动，因为这会提供更好的奖励。这意味着如果可能的话，机器人将采取更大的转弯来转身，这将增加总路径长度。但它通常会给机器人带来非常平滑的运动。不过，可以通过对转弯进行较小的惩罚来最小化转弯的大小。
此外，我们还添加了由 lambda 函数计算的术语 r3。如果机器人距离任何障碍物的距离小于 1 米，这会产生额外的负面奖励。使用这种“排斥”会使机器人对障碍物感到厌倦，并以更大的间隙绕过障碍物。
重置
如果一个事件已经结束（回想一下，一个事件是连续步骤的集合，直到碰撞、到达目标或达到最大步骤数），我们需要重置模拟。我们可以通过类函数重置来做到这一点。
def reset(self):
# Resets the state of the environment and returns an initial observation.
rospy.wait_for_service("/gazebo/reset_world")
try:
self.reset_proxy()
except rospy.ServiceException as e:
print("/gazebo/reset_simulation service call failed")在这里，我们将仿真环境重置为其初始状态。
但是，我们希望每一集都以机器人的不同初始位置和姿势开始。这将使我们能够改变机器人将遇到的体验，并更好地概括我们的网络。
angle = np.random.uniform(-np.pi, np.pi)
quaternion = Quaternion.from_euler(0.0, 0.0, angle)
object_state = self.set_self_state
x = 0
y = 0
position_ok = False
while not position_ok:
x = np.random.uniform(-4.5, 4.5)
y = np.random.uniform(-4.5, 4.5)
position_ok = check_pos(x, y)
object_state.pose.position.x = x
object_state.pose.position.y = y
# object_state.pose.position.z = 0.
object_state.pose.orientation.x = quaternion.x
object_state.pose.orientation.y = quaternion.y
object_state.pose.orientation.z = quaternion.z
object_state.pose.orientation.w = quaternion.w
self.set_state.publish(object_state)
self.odom_x = object_state.pose.position.x
self.odom_y = object_state.pose.position.y出于这个原因，我们对一个随机的欧拉角值进行采样，并将其转换为四元数。这将作为随机的机器人方向。之后，我们需要随机抽取 X 和 Y 坐标作为剧集的机器人起始位置。我们的模拟环境是一个 10 x 10 米的围墙部分。这意味着，我们希望机器人在这个围墙区域内随机生成。因此，我们希望随机值在环境原点周围的 -4.5 到 4.5 米范围内（与墙壁保持 0.5 米的距离）。但是我们在环境中还有其他障碍物，我们不想在它们上面生成我们的机器人。因此，我们随机选择 X 和 Y 坐标，直到它们不在障碍物周围的“死区”内。现在我们有了坐标和航向，我们更新了机器人位置并将其放置在set_state发布者那里。最后，将坐标保存为里程计信息。
通过调用一个函数在环境中随机放置一个目标，我们稍后将详细解释该函数。
# set a random goal in empty space in environment
self.change_goal()
# randomly scatter boxes in the environment
self.random_box()
self.publish_markers([0.0, 0.0])我们在模拟中放置了 4 个纸板箱。为了进一步改变每一集的环境，我们也可以在每集开始时随机改变它们的位置。我们还将详细解释这个功能。现在，我们只需重置发布操作信息的标记。由于我们在重置环境时不执行任何操作，因此我们可以简单地在此处传递零作为操作值。
将机器人、目标和盒子放置在环境中后，我们可以观察它恢复到第一状态。观察结果与我们在阶跃函数中已经看到的非常相似。
rospy.wait_for_service("/gazebo/pause_physics")
try:
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
v_state = []
v_state[:] = self.velodyne_data[:]
laser_state = [v_state]
distance = np.linalg.norm(
[self.odom_x - self.goal_x, self.odom_y - self.goal_y]
)
skew_x = self.goal_x - self.odom_x
skew_y = self.goal_y - self.odom_y
dot = skew_x * 1 + skew_y * 0
mag1 = math.sqrt(math.pow(skew_x, 2) + math.pow(skew_y, 2))
mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
beta = math.acos(dot / (mag1 * mag2))
if skew_y < 0:
if skew_x < 0:
beta = -beta
else:
beta = 0 - beta
theta = beta - angle
if theta > np.pi:
theta = np.pi - theta
theta = -np.pi - theta
if theta < -np.pi:
theta = -np.pi - theta
theta = np.pi - theta
robot_state = [distance, theta, 0.0, 0.0]
state = np.append(laser_state, robot_state)
return state再次传播状态以从我们的订阅者那里获取值。进行计算，将激光和机器人状态信息结合起来，并返回状态。
在这里，我们使用了 3 个附加函数，首先是change_goal。
def change_goal(self):
# Place a new goal and check if its location is not on one of the obstacles
if self.upper < 10:
self.upper += 0.004
if self.lower > -10:
self.lower -= 0.004
goal_ok = False
while not goal_ok:
self.goal_x = self.odom_x + random.uniform(self.upper, self.lower)
self.goal_y = self.odom_y + random.uniform(self.upper, self.lower)
goal_ok = check_pos(self.goal_x, self.goal_y)我们在初始化期间设置了类变量的上限和下限。上限和下限值描述了可以随机放置目标的范围。这样做是为了让我们使用一个简单的“引导”方法，帮助网络了解它需要导航到目标。通过在训练的早期将目标放在机器人附近，它有更高的机会偶然发现它。这意味着它将收集更多导航到目标的经验，这将有助于网络从良好的样本中学习。因此，在放置随机目标时，我们将其放置在随机放置的机器人位置附近，范围从下到上。 每次我们设定新目标时，我们都会增加下限和上限。但是在接受目标位置之前，这里我们还需要检查它是否没有放置在任何已知的障碍物上。
接下来，我们有 random_box 函数，它将盒子放置在环境中的随机位置。
def random_box(self):
# Randomly change the location of the boxes in the environment on each reset to randomize the training
# environment
for i in range(4):
name = "cardboard_box_" + str(i)
x = 0
y = 0
box_ok = False
while not box_ok:
x = np.random.uniform(-6, 6)
y = np.random.uniform(-6, 6)
box_ok = check_pos(x, y)
distance_to_robot = np.linalg.norm([x - self.odom_x, y - self.odom_y])
distance_to_goal = np.linalg.norm([x - self.goal_x, y - self.goal_y])
if distance_to_robot < 1.5 or distance_to_goal < 1.5:
box_ok = False
box_state = ModelState()
box_state.model_name = name
box_state.pose.position.x = x
box_state.pose.position.y = y
box_state.pose.position.z = 0.0
box_state.pose.orientation.x = 0.0
box_state.pose.orientation.y = 0.0
box_state.pose.orientation.z = 0.0
box_state.pose.orientation.w = 1.0
self.set_state.publish(box_state)这里的逻辑与change_goal函数非常相似，只是我们还想检查盒子的位置是否离机器人或目标不太近。对于我们环境中的所有 4 个盒子，这是按顺序完成的。
我们还有publish_markers功能，它可以在 Rviz 中发布动作值和目标位置的视觉信息。
def publish_markers(self, action):
# Publish visual data in Rviz
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "odom"
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.01
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = self.goal_x
marker.pose.position.y = self.goal_y
marker.pose.position.z = 0
markerArray.markers.append(marker)
self.publisher.publish(markerArray)
markerArray2 = MarkerArray()
marker2 = Marker()
marker2.header.frame_id = "odom"
marker2.type = marker.CUBE
marker2.action = marker.ADD
marker2.scale.x = abs(action[0])
marker2.scale.y = 0.1
marker2.scale.z = 0.01
marker2.color.a = 1.0
marker2.color.r = 1.0
marker2.color.g = 0.0
marker2.color.b = 0.0
marker2.pose.orientation.w = 1.0
marker2.pose.position.x = 5
marker2.pose.position.y = 0
marker2.pose.position.z = 0
markerArray2.markers.append(marker2)
self.publisher2.publish(markerArray2)
markerArray3 = MarkerArray()
marker3 = Marker()
marker3.header.frame_id = "odom"
marker3.type = marker.CUBE
marker3.action = marker.ADD
marker3.scale.x = abs(action[1])
marker3.scale.y = 0.1
marker3.scale.z = 0.01
marker3.color.a = 1.0
marker3.color.r = 1.0
marker3.color.g = 0.0
marker3.color.b = 0.0
marker3.pose.orientation.w = 1.0
marker3.pose.position.x = 5
marker3.pose.position.y = 0.2
marker3.pose.position.z = 0
markerArray3.markers.append(marker3)
self.publisher3.publish(markerArray3)我们需要看的最后一个函数是check_pos。此函数仅检查传入的 X 和 Y 值是否位于环境中任何障碍物的范围内。
# Check if the random goal position is located on an obstacle and do not accept it if it is
def check_pos(x, y):
goal_ok = True
if -3.8 > x > -6.2 and 6.2 > y > 3.8:
goal_ok = False
if -1.3 > x > -2.7 and 4.7 > y > -0.2:
goal_ok = False
if -0.3 > x > -4.2 and 2.7 > y > 1.3:
goal_ok = False
if -0.8 > x > -4.2 and -2.3 > y > -4.2:
goal_ok = False
if -1.3 > x > -3.7 and -0.8 > y > -2.7:
goal_ok = False
if 4.2 > x > 0.8 and -1.8 > y > -3.2:
goal_ok = False
if 4 > x > 2.5 and 0.7 > y > -3.2:
goal_ok = False
if 6.2 > x > 3.8 and -3.3 > y > -4.2:
goal_ok = False
if 4.2 > x > 1.3 and 3.7 > y > 1.5:
goal_ok = False
if -3.0 > x > -7.2 and 0.5 > y > -1.5:
goal_ok = False
if x > 4.5 or x < -4.5 or y > 4.5 or y < -4.5:
goal_ok = False
return goal_ok注： 这些范围是手动确定的，特定于存储库中使用的默认映射。如果您想使用任何其他地图进行训练，您需要在此函数中手动输入障碍物周围的“死区”范围。
在 第 5 部分中，我们将不再查看代码，但我们将介绍一些关于如何加快训练速度的提示和技巧以及其他杂项信息
969
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
