Webots仿真添加行人的走路模型，并且添加自定义ROS接口。
前言
最近做的导航项目仿真中需要在webots中测试动态避障效果，那么就需要在仿真环境中添加动态行人。幸运的是，webots基础库里已经提供了行人的模型和简单的控制程序，但是还需要将其与ros通信建立连接。自己也是摸索了很长时间，总算跑成功了。
大体可以分为两个部分：模型的添加和控制器的改造。我的webots版本是2023B,我的ROS2版本是humble。不同版本可能细节会有所不同，不过方法应该是一样的，我把我的过程记录分享一下，希望能帮到有同样困惑的同学。
第一步： webots 引入行人模型
点击"+"号导入模型
搜索"pedestrain" ,选择"humans->pedestrain->Pedestrain(robot)“”,点击add。
添加完成后，在左边可以看到行人pedestrain的各种属性。可以根据自己的喜好任意修改，我对这些属性不是很在意就是没有修改
我们可以看到这个模型自带默认控制器, 即"controller" 选项中“pedestrain” 我们可以点击edit看看这个默认的控制器是怎么写的，强烈建议花点时间看看这个py文件，下面我们改写的控制器也是基于这个文件。
那么如何使用这个默认控制器呢？ 从他的py文件中我们可以看到，需要三个参数trajectory，speed，step"。 speed默认0.5m/s，step默认和仿真一致，这两个参数一般不用改。需要改trajectory参数，表示行人的起点和终点位置.
双击"controllerArgs" 添加"–trajectory=5 10, 2 0"，表示起点坐标(5 10) 终点坐标(2 0)。
添加后保存，就可以看到行人模型在起点和终点之间来回巡航啦~
第二步： 控制器改造，可以跟ROS通信.
目前为止我们已经成功让出行人在webots里动起来了，可以要测试避障算法，我们需要知道行人的实时位置，所以下一步就是要改造控制器，让其可以跟ros通信，将行人的质心坐标试试发布出来。
安装webots_ros2
这个包如果用C++控制器需要安装，用python的话应该不需要。但是为了全面推荐大家事先都安装，安装方法很简单，直接clone后编译就行。或者apt安装
地址： https://github.com/cyberbotics/webots_ros2
新建控制器
cd 你的仿真文件目录
cd controllers
mkdir -p walk
cd walk
touch walk.py
chomd +x walk.py
上面我新建了一个"walk"控制器，名字随意，大家按照自己的喜好更改即可。
控制器修改
编辑walk.py，将下面的程序复制进去.
# Copyright 1996-2023 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Pedestrian class container."""
from controller import Supervisor
import optparse
import math
import rclpy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Quaternion, Twist, Pose
from builtin_interfaces.msg import Time
import tf2_ros
class Pedestrian(Supervisor):
"""Control a Pedestrian PROTO and publish its position using ROS 2."""
def __init__(self):
"""Constructor: initialize constants and ROS 2 setup."""
# Webots constants and variables
self.BODY_PARTS_NUMBER = 13
self.WALK_SEQUENCES_NUMBER = 8
self.ROOT_HEIGHT = 1.27
self.CYCLE_TO_DISTANCE_RATIO = 0.22
self.speed = 1.15
self.current_height_offset = 0
self.joints_position_field = []
self.joint_names = [
"leftArmAngle", "leftLowerArmAngle", "leftHandAngle",
"rightArmAngle", "rightLowerArmAngle", "rightHandAngle",
"leftLegAngle", "leftLowerLegAngle", "leftFootAngle",
"rightLegAngle", "rightLowerLegAngle", "rightFootAngle",
"headAngle"
]
self.height_offsets = [
-0.02, 0.04, 0.08, -0.03, -0.02, 0.04, 0.08, -0.03
]
self.angles = [
[-0.52, -0.15, 0.58, 0.7, 0.52, 0.17, -0.36, -0.74], # left arm
[0.0, -0.16, -0.7, -0.38, -0.47, -0.3, -0.58, -0.21], # left lower arm
[0.12, 0.0, 0.12, 0.2, 0.0, -0.17, -0.25, 0.0], # left hand
[0.52, 0.17, -0.36, -0.74, -0.52, -0.15, 0.58, 0.7], # right arm
[-0.47, -0.3, -0.58, -0.21, 0.0, -0.16, -0.7, -0.38], # right lower arm
[0.0, -0.17, -0.25, 0.0, 0.12, 0.0, 0.12, 0.2], # right hand
[-0.55, -0.85, -1.14, -0.7, -0.56, 0.12, 0.24, 0.4], # left leg
[1.4, 1.58, 1.71, 0.49, 0.84, 0.0, 0.14, 0.26], # left lower leg
[0.07, 0.07, -0.07, -0.36, 0.0, 0.0, 0.32, -0.07], # left foot
[-0.56, 0.12, 0.24, 0.4, -0.55, -0.85, -1.14, -0.7], # right leg
[0.84, 0.0, 0.14, 0.26, 1.4, 1.58, 1.71, 0.49], # right lower leg
[0.0, 0.0, 0.42, -0.07, 0.07, 0.07, -0.07, -0.36], # right foot
[0.18, 0.09, 0.0, 0.09, 0.18, 0.09, 0.0, 0.09] # head
]
Supervisor.__init__(self)
# ROS 2 setup
rclpy.init(args=None)
self.node = rclpy.create_node('pedestrian_controller')
self.publisher = self.node.create_publisher(Odometry, '/human_pose', 10)
# For twist and odometry
self.previous_position = [0, 0]
self.previous_time = 0
self.previous_angle = 0 # Initialize previous_angle to 0
def run(self):
"""Set the Pedestrian pose and position, and publish position."""
opt_parser = optparse.OptionParser()
opt_parser.add_option("--trajectory", default="", help="Specify the trajectory in the format [x1 y1, x2 y2, ...]")
opt_parser.add_option("--speed", type=float, default=0.3, help="Specify walking speed in [m/s]")
opt_parser.add_option("--step", type=int, help="Specify time step (otherwise world time step is used)")
options, args = opt_parser.parse_args()
if not options.trajectory or len(options.trajectory.split(',')) < 2:
print("You should specify the trajectory using the '--trajectory' option.")
print("The trajectory should have at least 2 points.")
return
if options.speed and options.speed > 0:
self.speed = options.speed
if options.step and options.step > 0:
self.time_step = options.step
else:
self.time_step = int(self.getBasicTimeStep())
point_list = options.trajectory.split(',')
self.number_of_waypoints = len(point_list)
self.waypoints = []
for i in range(0, self.number_of_waypoints):
self.waypoints.append([])
self.waypoints[i].append(float(point_list[i].split()[0]))
self.waypoints[i].append(float(point_list[i].split()[1]))
self.root_node_ref = self.getSelf()
self.root_translation_field = self.root_node_ref.getField("translation")
self.root_rotation_field = self.root_node_ref.getField("rotation")
for i in range(0, self.BODY_PARTS_NUMBER):
self.joints_position_field.append(self.root_node_ref.getField(self.joint_names[i]))
# compute waypoints distance
self.waypoints_distance = []
for i in range(0, self.number_of_waypoints):
x = self.waypoints[i][0] - self.waypoints[(i + 1) % self.number_of_waypoints][0]
y = self.waypoints[i][1] - self.waypoints[(i + 1) % self.number_of_waypoints][1]
if i == 0:
self.waypoints_distance.append(math.sqrt(x * x + y * y))
else:
self.waypoints_distance.append(self.waypoints_distance[i - 1] + math.sqrt(x * x + y * y))
while not self.step(self.time_step) == -1:
time = self.getTime()
current_sequence = int(((time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO) % self.WALK_SEQUENCES_NUMBER)
ratio = (time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO - \
int(((time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO))
for i in range(0, self.BODY_PARTS_NUMBER):
current_angle = self.angles[i][current_sequence] * (1 - ratio) + \
self.angles[i][(current_sequence + 1) % self.WALK_SEQUENCES_NUMBER] * ratio
self.joints_position_field[i].setSFFloat(current_angle)
# adjust height
self.current_height_offset = self.height_offsets[current_sequence] * (1 - ratio) + \
self.height_offsets[(current_sequence + 1) % self.WALK_SEQUENCES_NUMBER] * ratio
# move everything
distance = time * self.speed
relative_distance = distance - int(distance / self.waypoints_distance[self.number_of_waypoints - 1]) * \
self.waypoints_distance[self.number_of_waypoints - 1]
for i in range(0, self.number_of_waypoints):
if self.waypoints_distance[i] > relative_distance:
break
distance_ratio = 0
if i == 0:
distance_ratio = relative_distance / self.waypoints_distance[0]
else:
distance_ratio = (relative_distance - self.waypoints_distance[i - 1]) / \
(self.waypoints_distance[i] - self.waypoints_distance[i - 1])
x = distance_ratio * self.waypoints[(i + 1) % self.number_of_waypoints][0] + \
(1 - distance_ratio) * self.waypoints[i][0]
y = distance_ratio * self.waypoints[(i + 1) % self.number_of_waypoints][1] + \
(1 - distance_ratio) * self.waypoints[i][1]
root_translation = [x, y, self.ROOT_HEIGHT + self.current_height_offset]
angle = math.atan2(self.waypoints[(i + 1) % self.number_of_waypoints][1] - self.waypoints[i][1],
self.waypoints[(i + 1) % self.number_of_waypoints][0] - self.waypoints[i][0])
rotation = [0, 0, 1, angle]
self.root_translation_field.setSFVec3f(root_translation)
self.root_rotation_field.setSFRotation(rotation)
# Calculate velocity (twist)
velocity_x = (x - self.previous_position[0]) / (time - self.previous_time) if time != self.previous_time else 0
velocity_y = (y - self.previous_position[1]) / (time - self.previous_time) if time != self.previous_time else 0
angular_z = (angle - self.previous_angle) / (time - self.previous_time) if time != self.previous_time else 0
twist_msg = Twist()
twist_msg.linear.x = velocity_x
twist_msg.linear.y = velocity_y
twist_msg.angular.z = angular_z
# Publish Odometry message
odom_msg = Odometry()
odom_msg.header.stamp = Time() # Create a Time message
odom_msg.header.stamp.sec = int(time) # Set seconds
odom_msg.header.stamp.nanosec = int((time - int(time)) * 1e9) # Set nanoseconds
odom_msg.header.frame_id = "world"
# Correct initialization of Pose and Quaternion
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
odom_msg.pose.pose.orientation.z = math.sin(angle / 2)
odom_msg.pose.pose.orientation.w = math.cos(angle / 2)
odom_msg.twist.twist = twist_msg
self.publisher.publish(odom_msg)
# Store previous values for next iteration
self.previous_position = [x, y]
self.previous_time = time
self.previous_angle = angle # Store the angle for next iteration
# Shutdown ROS 2 node when controller ends
self.node.destroy_node()
rclpy.shutdown()
# Run the controller
controller = Pedestrian()
controller.run()
这个程序是基于上面的默认‘‘pedestrain’控制器修改的。主要修改了:
-
ROS 2 初始化：在 init 构造函数中初始化 ROS 2 节点和发布器。
– 使用 rclpy.init() 来初始化 ROS 2。
– 创建 rclpy.create_node(‘pedestrian_controller’) 来创建节点，并使用 node.create_publisher() 创建发布器 -
发布位置：在 run() 方法的 while 循环中，每次更新位置后，通过 self.publisher.publish(pose_msg) 将行人的位置信息发布到 ROS 2 的 /human_pose 话题。
-
控制器结束时，通过 self.node.destroy_node() 销毁 ROS 2 节点，并通过 rclpy.shutdown() 关闭 ROS 2。
参考链接
https://www.youtube.com/watch?v=_bVGHFBEOOA&t=9s