克隆后还缺仿真安装树和两边的编译目录。把 build、build_sim、install_sim 一并放进仓库；仿真入口不再写死本机 conda，build 里指向源码的链接改成相对路径。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;AMENT_PREFIX_PATH;
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;PYTHONPATH;lib/python3.11/site-packages
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"
|
||||
@@ -0,0 +1,781 @@
|
||||
"""启动 Gazebo 仿真：世界 + 机器人 + 控制器。
|
||||
|
||||
用法（推荐走 scripts/run_sim_gz.sh，它负责先激活仿真环境）：
|
||||
ros2 launch arm_sim_gazebo gazebo_sim.launch.py
|
||||
ros2 launch arm_sim_gazebo gazebo_sim.launch.py world:=/abs/other.world headless:=false
|
||||
|
||||
=============================================================================
|
||||
启动顺序为什么是这样（每一环都踩过）
|
||||
=============================================================================
|
||||
1. robot_state_publisher 发布归一化后的 URDF 到 /robot_description
|
||||
2. Gazebo（gz_sim） 加载世界
|
||||
3. ros_gz_sim create 从 /robot_description spawn 机器人
|
||||
4. controller spawner 拉起 joint_state_broadcaster + rizon_arm_controller
|
||||
|
||||
两处顺序是硬要求，颠倒了只表现为"静默不工作"：
|
||||
|
||||
a) RSP 必须在 create 之前就绪
|
||||
create 用 -topic robot_description 取 URDF。RSP 以 transient_local 发布
|
||||
（latched），迟到的订阅者也能拿到，所以这里主要是容忍，不是硬依赖。
|
||||
|
||||
b) controller spawner 必须在 Gazebo 起来之后
|
||||
controller_manager **跑在 Gazebo 进程里**（由 gz_ros2_control 的系统插件
|
||||
拉起），不是独立的 ros2_control_node。Gazebo 没起来时 spawner 找不到
|
||||
/controller_manager 服务，会一直重试直到 timeout。
|
||||
|
||||
c) 归一化后的 URDF 必须同时喂给 RSP 和 create
|
||||
gz_ros2_control 是从 /robot_description 读 <ros2_control> 段落的
|
||||
（SDF 里只存插件名和参数路径，接口声明不在 SDF —— 实测确认）。
|
||||
两边不是同一份时，控制器注册的关节名与 Gazebo 里的对不上，
|
||||
症状是 "Joint 'sim_joint1' not found in the robot model"。
|
||||
|
||||
=============================================================================
|
||||
use_sim_time
|
||||
=============================================================================
|
||||
仿真里所有节点都要 use_sim_time=True，时间来自 /clock（gz → ROS 桥接）。
|
||||
不设的话 TF 时间戳会用墙上时钟，与 Gazebo 的仿真时间不同源，
|
||||
表现为 RViz 里模型抖动、tf2 报 "Lookup would require extrapolation"。
|
||||
/clock 的桥接在下面 clock_bridge 里，故意排在最前面。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
IncludeLaunchDescription,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
TimerAction,
|
||||
)
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
# 启动延迟（秒）。都是为了避开"服务还没起来就去调"的竞态。
|
||||
# 数值偏保守：多等几秒远好过让用户对着一个静默失败的界面调试。
|
||||
SPAWN_DELAY = 4.0
|
||||
CONTROLLER_DELAY = 8.0
|
||||
|
||||
CONTROLLER_NAMES = ("joint_state_broadcaster", "rizon_arm_controller", "gripper_controller")
|
||||
|
||||
|
||||
def _prepare(context, *args, **kwargs):
|
||||
"""展开 + 归一化描述，并组装所有动作。
|
||||
|
||||
放在 OpaqueFunction 里做，因为需要先算出 URDF 文本才能构造
|
||||
robot_state_publisher 的参数 —— 而 launch 的 substitution 机制
|
||||
没法在 Python 里"看到"中间结果。
|
||||
"""
|
||||
from arm_sim_gazebo.robot_description import (
|
||||
SimDescriptionError,
|
||||
check_base_plugins,
|
||||
default_controllers,
|
||||
default_world,
|
||||
default_xacro,
|
||||
check_tactile_collisions,
|
||||
model_link_pose,
|
||||
prepare_urdf,
|
||||
repo_root,
|
||||
world_name,
|
||||
)
|
||||
from arm_sim_gazebo.sim_site import SimSiteError, resolve_world
|
||||
|
||||
world = LaunchConfiguration("world").perform(context).strip()
|
||||
load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower()
|
||||
with_sensors = LaunchConfiguration("with_sensors").perform(context).strip().lower()
|
||||
with_scene = LaunchConfiguration("with_scene").perform(context).strip().lower()
|
||||
scene_file = LaunchConfiguration("scene").perform(context).strip()
|
||||
headless = LaunchConfiguration("headless").perform(context).strip().lower()
|
||||
verbose = LaunchConfiguration("verbose").perform(context).strip().lower()
|
||||
|
||||
# 相机清单在下面的传感器分支里才读，但场景分支（成像几何自检）也要用它。
|
||||
# 先置空再赋值，避免"因为分支没进而在别处 NameError"这种低级但难查的错误。
|
||||
specs = []
|
||||
|
||||
try:
|
||||
root = repo_root()
|
||||
# 与 gazebo_sim_launcher 用同一套优先级，直接 `ros2 launch` 的人也不会
|
||||
# 遇到"改了 site.yaml 没反应"：--world > site.yaml 的 sim.world > 内置默认。
|
||||
world_path, _world_origin = resolve_world(world, root, fallback=default_world(root))
|
||||
world_path = str(world_path)
|
||||
|
||||
# 腕部 FT 的话题名先读出来，喂给 xacro —— 这个话题名有三处要对上
|
||||
# （配置、URDF 里的 <topic>、应用层按 sn 推出的 /{sn}/ft_sensor_wrench），
|
||||
# 让配置当唯一源头，xacro 不再自己写一份。
|
||||
from arm_sim_gazebo.sim_wrist_ft import (
|
||||
WristFtConfigError,
|
||||
bridge_args as wrist_ft_bridge_args,
|
||||
check_wrist_ft_sensor,
|
||||
)
|
||||
from arm_sim_gazebo.sim_wrist_ft import load_wrist_ft
|
||||
|
||||
try:
|
||||
wft = load_wrist_ft(root=root)
|
||||
except WristFtConfigError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 仿真腕部 FT 配置有问题：\n{exc}\n") from exc
|
||||
|
||||
# 抓取锁：仿真里唯一能保证"夹住的东西不会在机械臂连续运动时滑掉"的机制，
|
||||
# 说明见 devices/sim/gazebo/config/grasp_lock.yaml 与 sim_grasp_lock.py。
|
||||
from arm_sim_gazebo.sim_grasp_lock import (
|
||||
DEFAULT_OBJECT_SDF_TMPL,
|
||||
GraspLockError,
|
||||
check_grasp_lock_sdf,
|
||||
)
|
||||
from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock
|
||||
|
||||
try:
|
||||
glock = load_grasp_lock(root=root)
|
||||
except GraspLockError as exc:
|
||||
raise RuntimeError(
|
||||
f"\n[gazebo_sim] 仿真抓取锁配置有问题：\n{exc}\n"
|
||||
f" （只想先跑起来的话，把 grasp_lock.yaml 的 enabled 改成 false）\n"
|
||||
) from exc
|
||||
|
||||
# 静态核对：物体 SDF 里到底有没有那个插件、参数对不对得上。
|
||||
# 差一处就**静默失效**（现象只是"规莫名掉了"），所以必须在这里说清楚。
|
||||
glock_warnings: List[str] = []
|
||||
if glock.enabled:
|
||||
_obj_sdf = root / DEFAULT_OBJECT_SDF_TMPL.format(object=glock.object)
|
||||
try:
|
||||
_sdf_text = _obj_sdf.read_text(encoding="utf-8")
|
||||
except OSError:
|
||||
_sdf_text = ""
|
||||
glock_warnings = check_grasp_lock_sdf(
|
||||
_sdf_text, glock, sdf_label=str(_obj_sdf)
|
||||
)
|
||||
|
||||
urdf, report = prepare_urdf(
|
||||
xacro_path=default_xacro(root),
|
||||
controllers_file=default_controllers(root),
|
||||
load_gripper=load_gripper != "false",
|
||||
with_sensors=with_sensors != "false",
|
||||
wrist_ft_topic=wft.topic,
|
||||
)
|
||||
wname = world_name(world_path)
|
||||
missing_base = check_base_plugins(Path(world_path))
|
||||
except (SimDescriptionError, SimSiteError) as exc:
|
||||
# 描述准备失败是最常见的上手障碍（环境没 source、路径不对）。
|
||||
# 直接把可读原因打出来，而不是让它变成一个看不懂的 launch traceback。
|
||||
raise RuntimeError(f"\n[gazebo_sim] 仿真描述准备失败：\n{exc}\n") from exc
|
||||
|
||||
actions = [
|
||||
LogInfo(msg=f"[gazebo_sim] 仓库根: {root}"),
|
||||
LogInfo(msg=f"[gazebo_sim] 世界: {world_path} (world 名: {wname})"),
|
||||
LogInfo(msg=f"[gazebo_sim] URDF 归一化: {report.summary()}"),
|
||||
LogInfo(msg=f"[gazebo_sim] {glock.describe()}"),
|
||||
]
|
||||
for _w in glock_warnings:
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 抓取锁检查: {_w}"))
|
||||
if missing_base:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=(
|
||||
"[gazebo_sim] ⚠ world 缺少基础系统: "
|
||||
+ ", ".join(missing_base)
|
||||
+ "。world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认"
|
||||
"系统列表。缺 UserCommands 会让 /world/<名>/create 服务不存在，"
|
||||
"模型根本不会被创建，而 create 仍打印 'OK creation of entity' ——"
|
||||
"最终表现是控制器永远拉不起来。修法见 assembly.world 里的注释。"
|
||||
)
|
||||
)
|
||||
)
|
||||
if not report.changed:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] 注意：归一化没有改动任何内容。若 Gazebo 仍报 link 不存在，"
|
||||
"先确认 flexiv_description 的版本是否已自行修好这两个上游缺陷。"
|
||||
)
|
||||
)
|
||||
|
||||
# --- 1) 机器人描述 -------------------------------------------------------
|
||||
# 显式传 URDF 字符串（而不是 Command(["xacro ..."])），因为要发的是
|
||||
# **归一化后**的那一份。这是 RSP 与 create 必须一致的东西，见文件头 (c)。
|
||||
actions.append(
|
||||
Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{"robot_description": urdf},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
# --- 2) Gazebo -----------------------------------------------------------
|
||||
gz_args = ["-r"] # 立刻开始推进仿真（不等 GUI 里的播放按钮）
|
||||
if verbose == "true":
|
||||
gz_args += ["-v", "4"] # 打开 Gazebo 自身日志，排查插件加载失败时必需
|
||||
if headless == "true":
|
||||
# -s = 只跑服务端，不开 GUI。无显示器环境（本机就是）必须用这个。
|
||||
gz_args.append("-s")
|
||||
gz_args.append(world_path)
|
||||
|
||||
actions.append(
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(
|
||||
FindPackageShare("ros_gz_sim").perform(context),
|
||||
"launch",
|
||||
"gz_sim.launch.py",
|
||||
)
|
||||
),
|
||||
launch_arguments={"gz_args": " ".join(gz_args)}.items(),
|
||||
)
|
||||
)
|
||||
|
||||
# --- 3) /clock 桥接（sim time 的来源，排在最前）-------------------------
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="clock_bridge",
|
||||
arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
|
||||
# --- 3b) 世界模型位姿回读：物体被撞走时唯一能指名报错的数据源 ------------
|
||||
# 为什么值得一条独立桥接：`perception.source=truth` 是**纯几何**的，
|
||||
# 它按 scene.yaml 的**名义位姿**算孔位，从不问 Gazebo 物体现在在哪。
|
||||
# 工件被碰走（实测：掉进台面通槽落到地面）之后，"真值"开始撒谎，
|
||||
# 而现象全都指向别处（S05 报"两孔间距 169mm"、S07 一路下压不接触）。
|
||||
# 这一条把 gz 的"每个模型实际位姿"搬进 ROS，让 sim_truth_perception
|
||||
# 能把它和 scene.yaml 对上、指名报"工件不在它该在的地方"。
|
||||
# 具体参数（话题名/类型/漂移容差）在 scene.yaml 的 pose_info 段。
|
||||
from arm_sim_gazebo.sim_scene import load_scene as _load_scene
|
||||
|
||||
try:
|
||||
_spec = _load_scene(None, root)
|
||||
except Exception as exc: # noqa: BLE001 —— 场景读不出来要说清是场景的问题
|
||||
raise RuntimeError(f"\n[gazebo_sim] 场景配置有问题：\n{exc}\n") from exc
|
||||
if _spec.pose_info is not None:
|
||||
_pi = _spec.pose_info
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="pose_info_bridge",
|
||||
output="screen",
|
||||
arguments=[f"{_pi.gz_topic}@{_pi.ros_type}[{_pi.gz_type}"],
|
||||
# gz 的话题名固定是 /world/<world>/pose/info（命名空间由 gz 决定，
|
||||
# 应用侧改不了），ros_topic 只是"到 ROS 这一侧叫什么"。
|
||||
# ros_gz_bridge 没有改名能力，所以这里用 remap 把名字对到
|
||||
# scene.yaml 声明的那一个 —— 两侧对不上的表现是
|
||||
# "桥接器起来了但一个消息都没有"，那是个假故障。
|
||||
remappings=[(_pi.gz_topic, _pi.ros_topic)],
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=f"[gazebo_sim] 世界模型位姿回读: {_pi.gz_topic} → {_pi.ros_topic}"
|
||||
)
|
||||
)
|
||||
|
||||
# --- 4) 相机：桥接 → 适配器 → （世界相机）补 TF --------------------------
|
||||
# 只在挂了传感器时才做。--no-sensors 时跳过是必要的：适配器会去订阅
|
||||
# 永远不存在的 gz 话题，它**不会报错**，只是静默没输出 —— 而"相机没图"
|
||||
# 正是最需要快速定位的问题，不该被一个假故障混淆。
|
||||
if with_sensors != "false":
|
||||
from arm_sim_gazebo.sim_cameras import (
|
||||
DEFAULT_CAMERAS_REL,
|
||||
CameraConfigError,
|
||||
bridge_args,
|
||||
check_camera_consistency,
|
||||
load_cameras,
|
||||
rpy_to_quat,
|
||||
)
|
||||
|
||||
try:
|
||||
specs = load_cameras(root=root)
|
||||
except CameraConfigError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 仿真相机清单有问题：\n{exc}\n") from exc
|
||||
|
||||
# 一致性：内参由 cameras.yaml 的 horizontal_fov 与 width/height 算，
|
||||
# 而**渲染**用的是 SDF 里的值。两者不一致时图像与内参对不上，
|
||||
# 三维点位会整体错 —— 而单目预览完全看不出来（gz 自己发的
|
||||
# camera_info 本来就是错的，所以这份内参是"唯一真相"，更要保证它与渲染一致）。
|
||||
# 图幅尤其要查：它决定 fx 的尺度，写错会让孔检测的像素门槛静默失效。
|
||||
# 这里只告警不拦截：真有偏差时，仍应能起仿真去现场比对。
|
||||
_cam_issues = check_camera_consistency(
|
||||
specs,
|
||||
world_file=Path(world_path),
|
||||
sensors_xacro=Path(root)
|
||||
/ "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
|
||||
)
|
||||
# ⚠ 这里用 LogInfo 而不是 LogWarn：Humble 的 launch.actions 里没有
|
||||
# LogWarn（Iron 才加），照抄新版本会直接 ImportError 让整个 launch
|
||||
# 加载失败 —— 一个"只是想说句话"的动作把仿真起不来，代价太大。
|
||||
# 前缀里带 ⚠ 保证它在满屏日志里仍然显眼。
|
||||
for _w in _cam_issues:
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ {_w}"))
|
||||
|
||||
# 4a) 世界相机的 TF 兜底
|
||||
#
|
||||
# Gazebo **不会**为 world 里的 <model> 产生 ROS TF —— TF 只来自
|
||||
# robot_state_publisher（也就是只覆盖 URDF 里的东西）。
|
||||
# D435 正属于这一类：它的物理归属是"世界"（固定支架），不在 URDF 里
|
||||
# （早期挂在 URDF 的 world link 上时，被 sdformat 连整棵子树静默丢掉）。
|
||||
# 少了这条 TF，应用 tf2 查询直接报 "frame does not exist" ——
|
||||
# 而图像本身完全正常，很容易误判成相机坏了。
|
||||
#
|
||||
# 位姿从 world 文件**解析**，不在这里重复配置：唯一真相就是 world 文件。
|
||||
# 另配一份等于制造"改了 world 忘了改这里"，而症状是相机看到的东西
|
||||
# 与它自称的位置对不上，极难查。
|
||||
world_tf = []
|
||||
for s in specs:
|
||||
if not s.is_world_mounted:
|
||||
continue
|
||||
try:
|
||||
(px, py, pz), (rr, rp, ry) = model_link_pose(
|
||||
Path(world_path), s.world_model, s.world_link
|
||||
)
|
||||
except SimDescriptionError as exc:
|
||||
raise RuntimeError(
|
||||
f"\n[gazebo_sim] 世界相机 {s.ros_ns} 的 TF 补不出来：\n{exc}\n"
|
||||
) from exc
|
||||
qx, qy, qz, qw = rpy_to_quat((rr, rp, ry))
|
||||
world_tf.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name=f"tf_{s.ros_ns}",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"--x", f"{px}", "--y", f"{py}", "--z", f"{pz}",
|
||||
"--qx", f"{qx}", "--qy", f"{qy}", "--qz", f"{qz}", "--qw", f"{qw}",
|
||||
"--frame-id", "world",
|
||||
"--child-frame-id", s.link_frame,
|
||||
],
|
||||
parameters=[{"use_sim_time": True}],
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=f"[gazebo_sim] 世界相机 {s.ros_ns}: world → {s.link_frame} "
|
||||
f"@ ({px:.3f}, {py:.3f}, {pz:.3f}) 由 world 文件解析得到"
|
||||
)
|
||||
)
|
||||
if world_tf:
|
||||
# 延后一点再发：/clock 还没起来时发布静态 TF 可能被 tf2 判成
|
||||
# "未来时间戳"，而这一类问题只在很后面才以 extrapolation 报出来。
|
||||
actions.append(TimerAction(period=2.0, actions=world_tf))
|
||||
|
||||
# 4b) 桥接：只做传输（gz 消息类型 → ROS 消息类型，话题名照抄）。
|
||||
# 改名、坐标系、深度单位都交给 4c 的适配器 —— 桥接器改不了 frame_id，
|
||||
# 而 frame_id 恰恰是最容易错又最不报错的那一项。
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="camera_bridge",
|
||||
output="screen",
|
||||
arguments=bridge_args(specs),
|
||||
)
|
||||
)
|
||||
|
||||
# 4c) 适配器：盖光学系 frame_id + 深度 32FC1 米 → 16UC1 毫米 + 改成
|
||||
# 应用期望的话题名。详见 sim_camera_adapter.py 顶部。
|
||||
actions.append(
|
||||
Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_camera_adapter",
|
||||
name="sim_camera_adapter",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{"cameras_file": str(Path(root) / DEFAULT_CAMERAS_REL)},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
# 4d) 触觉：桥接 + 适配器 -------------------------------------------
|
||||
# 链路与相机同构：gz 只负责"有数据"，语义（轴向、基线、单位、接触超时）
|
||||
# 全在适配器里做。见 sim_tactile.py / sim_tactile_adapter.py 顶部。
|
||||
from arm_sim_gazebo.sim_tactile import (
|
||||
DEFAULT_TACTILE_REL,
|
||||
TactileConfigError,
|
||||
)
|
||||
from arm_sim_gazebo.sim_tactile import bridge_args as tactile_bridge_args
|
||||
from arm_sim_gazebo.sim_tactile import load_tactile
|
||||
|
||||
try:
|
||||
tac = load_tactile(root=root)
|
||||
except TactileConfigError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 仿真触觉配置有问题：\n{exc}\n") from exc
|
||||
|
||||
# 描述级静态检查：接触传感器引用的 collision 名是 sdformat **自动生成**的，
|
||||
# 换个 sdformat 版本就可能变，而变了之后传感器只是"永远没输出"、不报错。
|
||||
# 这里把它和"传感器挂错关节""话题写在错误的层级""传感器被整个丢掉"
|
||||
# 一起在启动时报出来。expected_topics 用配置里声明的话题做对照 ——
|
||||
# 这是唯一能发现"传感器被静默丢弃"的办法。
|
||||
for _w in check_tactile_collisions(
|
||||
urdf,
|
||||
expected_topics=[s.wrench_topic for s in tac.sides]
|
||||
+ [s.contact_topic for s in tac.sides],
|
||||
):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 触觉描述检查: {_w}"))
|
||||
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="tactile_bridge",
|
||||
output="screen",
|
||||
arguments=tactile_bridge_args(tac),
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_tactile_adapter",
|
||||
name="sim_tactile_adapter",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{"tactile_file": str(Path(root) / DEFAULT_TACTILE_REL)},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
# 4e) 腕部 FT：只需桥接，**不需要适配器** ------------------------------
|
||||
# 与触觉不同：触觉要轴向映射/基线/单位换算/contact 折算，所以有适配器；
|
||||
# 腕部 FT 的 gz 输出本来就是牛顿、坐标系就是 child(link7) 系，而
|
||||
# ros_gz_bridge **原生支持** gz.msgs.Wrench → WrenchStamped（实测确认）。
|
||||
# 于是仿真产出的就是真机同名的 /<sn>/ft_sensor_wrench（同名同类型同单位），
|
||||
# 应用层一行分支都不用写。少一层节点就少一个"配错了不报错"的地方。
|
||||
if with_sensors != "false":
|
||||
for _w in check_wrist_ft_sensor(urdf, wft):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 腕部 FT 检查: {_w}"))
|
||||
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="wrist_ft_bridge",
|
||||
output="screen",
|
||||
arguments=wrist_ft_bridge_args(wft),
|
||||
)
|
||||
)
|
||||
|
||||
# --- 5) spawn 机器人 -----------------------------------------------------
|
||||
# -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
|
||||
# 表现成"launch 一切正常但 RViz 里什么都没有"。
|
||||
actions.append(
|
||||
TimerAction(
|
||||
period=SPAWN_DELAY,
|
||||
actions=[
|
||||
Node(
|
||||
package="ros_gz_sim",
|
||||
executable="create",
|
||||
name="spawn_robot",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"-world", wname,
|
||||
"-name", "rizon_sim",
|
||||
"-topic", "robot_description",
|
||||
],
|
||||
)
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
# --- 5b) spawn 场景物体（工件 / 通止规）---------------------------------
|
||||
# 场景物体**不进 world 文件**，而是在这里 spawn：
|
||||
# world 放"固定不动"的东西（地面、台子、支架相机），
|
||||
# 物料（工件、规）会按站点/工序换位置，写进 world 就等于每次换料都改 world，
|
||||
# 而 world 里还有物理参数与插件，动它的风险面太大（见 assembly.world 的注释）。
|
||||
#
|
||||
# ⚠️ 必须**等机器人 spawn 完**再 spawn 场景（这里再延后 2s）：
|
||||
# 两者都走 /world/<名>/create 服务，同时打会争同一个服务，
|
||||
# 重试窗口内失败的那个**不报错**（create 打印 OK 但实体没进），
|
||||
# 表现成"工件时有时无"，非常难查。
|
||||
if with_scene != "false":
|
||||
from arm_sim_gazebo.sim_cameras import intrinsics as _intrinsics
|
||||
from arm_sim_gazebo.sim_scene import (
|
||||
SceneError,
|
||||
check_pixel_gates,
|
||||
load_scene,
|
||||
operating_flange_z,
|
||||
operating_pixel_report,
|
||||
spawn_commands,
|
||||
validate_scene,
|
||||
)
|
||||
|
||||
try:
|
||||
scene = load_scene(
|
||||
Path(scene_file) if scene_file else None, root=root
|
||||
)
|
||||
except SceneError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 场景配置有问题：\n{exc}\n") from exc
|
||||
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] 场景: {scene.source}"))
|
||||
|
||||
# 摆放校验：悬空 / 穿模 / 出界 / 互相重叠 —— 全是"看起来正常、跑起来不对"
|
||||
# 的那一类。这里只告警不拦截：真有问题时仍需能起仿真去看现场。
|
||||
for _p in validate_scene(scene, root):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 场景摆放检查: {_p}"))
|
||||
|
||||
scene_nodes = []
|
||||
for args in spawn_commands(scene, root):
|
||||
name = args[args.index("-name") + 1]
|
||||
scene_nodes.append(
|
||||
Node(
|
||||
package="ros_gz_sim",
|
||||
executable="create",
|
||||
name=f"spawn_{name}",
|
||||
output="screen",
|
||||
arguments=["-world", wname] + args,
|
||||
)
|
||||
)
|
||||
if scene_nodes:
|
||||
actions.append(TimerAction(period=SPAWN_DELAY + 2.0, actions=scene_nodes))
|
||||
|
||||
# 成像几何自检：把"该被检测到的特征"在这台相机下换算成像素，
|
||||
# 与门槛比一遍。这是**唯一**能在上真机之前发现"孔间距不够像素门槛"
|
||||
# 的办法 —— 等真机上才发现要贵得多。
|
||||
cam = next((s for s in specs if s.ros_ns == scene.operating.camera), None) if with_sensors != "false" else None
|
||||
if cam is not None:
|
||||
_fx = _intrinsics(cam, cam.width, cam.height)[0][0]
|
||||
px = operating_pixel_report(scene, _fx, root)
|
||||
if px:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] 孔检测几何自检（相机 {}，到工件 {:.2f}m，fx={:.1f}px）：\n".format(
|
||||
scene.operating.camera, scene.operating.camera_to_part_m, _fx
|
||||
)
|
||||
+ "\n".join(f" {k} = {v:.1f}px" for k, v in sorted(px.items()))
|
||||
)
|
||||
)
|
||||
for _p in check_pixel_gates(px, scene.gates_px):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 孔检测几何: {_p}"))
|
||||
_fz = operating_flange_z(scene, root)
|
||||
if _fz is not None:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=f"[gazebo_sim] 二阶段在仿真里应悬停到法兰 z≈{_fz:.3f}m"
|
||||
f"（真机 site.yaml 写的是 0.23 —— 两者不同是**已知**的，"
|
||||
f"原因是两边「基座与台面的相对高度」不同；"
|
||||
f"决定成像的是相机到工件的距离，见 scene.yaml 的 operating 段）"
|
||||
)
|
||||
)
|
||||
else:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] 未做孔检测几何自检（缺相机清单，或 --no-sensors）"
|
||||
)
|
||||
)
|
||||
|
||||
# --- 6) 控制器 -----------------------------------------------------------
|
||||
# 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
|
||||
# rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
|
||||
controllers = [
|
||||
TimerAction(
|
||||
period=CONTROLLER_DELAY + i * 2.0,
|
||||
actions=[
|
||||
Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
name=f"spawn_{name}",
|
||||
output="screen",
|
||||
arguments=[
|
||||
name,
|
||||
"--controller-manager", "/controller_manager",
|
||||
# controller_manager 在 Gazebo 进程里，它是 sim time；
|
||||
# spawner 也要用 sim time，否则两者对时不同源。
|
||||
"--controller-manager-timeout", "120",
|
||||
],
|
||||
parameters=[{"use_sim_time": True}],
|
||||
)
|
||||
],
|
||||
)
|
||||
for i, name in enumerate(CONTROLLER_NAMES)
|
||||
]
|
||||
actions.extend(controllers)
|
||||
|
||||
# --- 6b) 仿真夹爪适配器 --------------------------------------------------
|
||||
# 为什么必须有它：sdformat 把 URDF 的 <mimic> 整个丢掉，gz-sim 也不实现，
|
||||
# 所以 6 个从动关节不会跟随主关节。适配器按耦合公式算出 7 个关节的目标值
|
||||
# 一起下发，并对外提供与真机**同名同类型**的 GripperExecute 服务
|
||||
#（<ns>/open|close|move|rotate + status），上层代码一行都不用改。
|
||||
#
|
||||
# 耦合表来自 report.stripped_mimics —— 也就是 urdf_normalize 删掉 <mimic>
|
||||
# 时记下来的那几张表。让 launch 传给适配器，而不是让适配器自己再展开一次
|
||||
# xacro：两次展开在参数不同时会静默不一致，而症状是"手指角度不对"。
|
||||
if load_gripper != "false":
|
||||
from arm_sim_gazebo.sim_gripper import format_mimic_param
|
||||
|
||||
mimics = [format_mimic_param(*t) for t in report.stripped_mimics]
|
||||
if not mimics:
|
||||
# 这里必须出声：拿不到耦合表说明夹爪没被加载（或上游改了耦合方式），
|
||||
# 而适配器会因此拒绝启动 —— 早知道比晚知道好。
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] ⚠ 归一化报告里没有 mimic 关系，"
|
||||
"仿真夹爪适配器将无法启动（它会明确报错）。"
|
||||
"检查 load_gripper 与 flexiv_description 的 grav 宏。"
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
TimerAction(
|
||||
# 等 gripper_controller 起来再发命令：控制器还没 active 时
|
||||
# 发出去的 /commands 会被丢弃（不报错），夹爪就一直停在初始位。
|
||||
period=CONTROLLER_DELAY + len(CONTROLLER_NAMES) * 2.0 + 1.0,
|
||||
actions=[
|
||||
Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_gripper_adapter",
|
||||
name="sim_gripper_adapter",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"controllers_file": str(default_controllers(root)),
|
||||
"controller": "gripper_controller",
|
||||
"mimics": mimics,
|
||||
"robot_description": urdf,
|
||||
"namespace": "/gripper",
|
||||
# 抓取锁（仿真专用，见 sim_grasp_lock.py）：
|
||||
# 夹住后把物体固定到手指上，避免"位置控制夹持 +
|
||||
# 连续轨迹"导致的打滑掉件。
|
||||
"grasp_lock_enabled": bool(glock.enabled),
|
||||
"grasp_lock_object": str(glock.object),
|
||||
"grasp_lock_attach_topic": str(glock.attach_topic),
|
||||
"grasp_lock_detach_topic": str(glock.detach_topic),
|
||||
"grasp_lock_timeout_ms": int(glock.publish_timeout_ms),
|
||||
},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
# --- 6c) 机械臂的"应用接口"对齐真机 ---------------------------------------
|
||||
# 仿真里唯一的控制通路是 ros2_control 的 JTC（control_msgs）。但应用层在
|
||||
# 真机上用的是另一层接口 —— 这里把它们**按真机的名字与类型**补齐，
|
||||
# 于是通止规装配（以及 stage2/stage3）在仿真里能不改一行代码就跑：
|
||||
#
|
||||
# /follow_joint_trajectory arm_msgs/FollowJointTrajectory → JTC
|
||||
# 真机由 real_trajectory_adapter 提供（名字带 real，但实现只做 action
|
||||
# 桥接、与硬件无关；这里**复用同一个节点**而不是再写一份 ——
|
||||
# 两份实现迟早会不一致）。
|
||||
# /set_named_pose arm_msgs/SetNamedPose（命名位姿）→ 上面那个
|
||||
# 真机由 arm_bringup/stack.launch.py 起，仿真起同一份，只是把
|
||||
# joint_prefix 换成 sim_（关节名不同，点位数值是同一台机器人）。
|
||||
# /home std_srvs/Trigger
|
||||
# 真机由 flexiv_arm_driver 提供（= 切工具系 + 转 /set_named_pose）。
|
||||
# 仿真没有 Flexiv 驱动，用 sim_home_service 补同一份契约（不切工具系）。
|
||||
actions.append(
|
||||
Node(
|
||||
package="arm_control",
|
||||
executable="real_trajectory_adapter",
|
||||
name="real_trajectory_adapter",
|
||||
output="screen",
|
||||
parameters=[{"use_sim_time": True}],
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
Node(
|
||||
package="arm_control",
|
||||
executable="named_pose_server",
|
||||
name="named_pose_server",
|
||||
output="screen",
|
||||
parameters=[
|
||||
# 命名位姿用**包内默认那份**（src/arm_control/config/named_poses.yaml）：
|
||||
# 它只有 home/ready/inspect 这几个点，与"仿真里是哪台臂"无关；
|
||||
# 真机走 site.yaml 的 robots[].paths.named_poses_file（那份带该臂的点位）。
|
||||
str(root / "src" / "arm_control" / "config" / "named_poses.yaml"),
|
||||
# 关节名：仿真里是 sim_joint1..7（前缀来自 urdf 的 arm_prefix，
|
||||
# site.yaml 的 sim.joint_prefix 目前是"声明了但没接线"，见那一段注释）。
|
||||
{"joint_prefix": "sim_"},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_home_service",
|
||||
name="sim_home_service",
|
||||
output="screen",
|
||||
parameters=[{"use_sim_time": True}],
|
||||
)
|
||||
)
|
||||
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=(
|
||||
"[gazebo_sim] 启动中。验收命令：\n"
|
||||
" ros2 topic echo /clock --once # 仿真时间在走\n"
|
||||
" ros2 control list_controllers # 两个控制器 active\n"
|
||||
" ros2 topic echo /joint_states --once # 7 个 sim_joint\n"
|
||||
" ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查\n"
|
||||
" ros2 topic echo /tactile/left/status --once # 触觉三力在发\n"
|
||||
" ign model -m workpiece -p # 工件在台面上（核对摆放）\n"
|
||||
" ign model -m gauge -p # 通止规立在台面上\n"
|
||||
" python3 scripts/check_sim_cameras.py # ★ 相机的真正验收\n"
|
||||
" python3 scripts/check_sim_scene.py # ★ 场景摆放 + 孔检测几何验收\n"
|
||||
" 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
|
||||
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n"
|
||||
" ★ 别用 'ros2 topic hz' 或肉眼预览来验收相机 —— 那些只能证明'有图像'。\n"
|
||||
" 仿真相机的错误几乎全是静默的（深度单位差 1000 倍、frame_id 盖成\n"
|
||||
" 本体系、gz 发的内参是错的、位姿 rpy 写错导致相机背对目标），\n"
|
||||
" 它们全都能出图、能查到 TF。必须用 check_sim_cameras.py 反投影\n"
|
||||
" 到 world、检查是否命中地面/台面，才算通过。\n"
|
||||
" ⚠ 深度话题是 16UC1 毫米（已从 gz 的 32FC1 米转换过）。\n"
|
||||
" 内参也不是 gz 发的那份：gz 恒发 fx=277/cx=160/cy=120（它的默认\n"
|
||||
" 60° 视场 + 半分辨率主点），适配器已按 cameras.yaml 的视场重算覆盖。\n"
|
||||
" 所以改相机视场要同时改 cameras.yaml 和 SDF，启动时会自动比对告警。"
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
return actions
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument(
|
||||
"world",
|
||||
default_value="",
|
||||
description="世界文件绝对路径。留空=用仓库里默认的 assembly.world",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"load_gripper",
|
||||
default_value="true",
|
||||
description="是否挂 GN01 夹爪。装配任务需要 true",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"with_sensors",
|
||||
default_value="true",
|
||||
description="是否挂 D405 与触觉。关掉跑得更快，用于纯运动学验证",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"with_scene",
|
||||
default_value="true",
|
||||
description="是否 spawn 场景物料（工件/通止规）。false=只用空台面",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"scene",
|
||||
default_value="",
|
||||
description="场景配置文件路径。留空=用 devices/sim/gazebo/config/scene.yaml",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"headless",
|
||||
default_value="true",
|
||||
description="true=只开 Gazebo 服务端（无显示器环境用）；false=带 GUI",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"verbose",
|
||||
default_value="false",
|
||||
description="true=打开 Gazebo 详细日志（排查插件加载失败时开）",
|
||||
),
|
||||
OpaqueFunction(function=_prepare),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,31 @@
|
||||
# generated from colcon_bash/shell/template/package.bash.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# a bash script is able to determine its own path if necessary
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
_colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
|
||||
else
|
||||
_colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_bash_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# source sh script of this package
|
||||
_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/package.sh"
|
||||
|
||||
unset _colcon_package_bash_source_script
|
||||
unset _colcon_package_bash_COLCON_CURRENT_PREFIX
|
||||
@@ -0,0 +1,6 @@
|
||||
source;share/arm_sim_gazebo/hook/pythonpath.ps1
|
||||
source;share/arm_sim_gazebo/hook/pythonpath.dsv
|
||||
source;share/arm_sim_gazebo/hook/pythonpath.sh
|
||||
source;share/arm_sim_gazebo/hook/ament_prefix_path.ps1
|
||||
source;share/arm_sim_gazebo/hook/ament_prefix_path.dsv
|
||||
source;share/arm_sim_gazebo/hook/ament_prefix_path.sh
|
||||
@@ -0,0 +1,116 @@
|
||||
# generated from colcon_powershell/shell/template/package.ps1.em
|
||||
|
||||
# function to append a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as leading separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
function colcon_append_unique_value {
|
||||
param (
|
||||
$_listname,
|
||||
$_value
|
||||
)
|
||||
|
||||
# get values from variable
|
||||
if (Test-Path Env:$_listname) {
|
||||
$_values=(Get-Item env:$_listname).Value
|
||||
} else {
|
||||
$_values=""
|
||||
}
|
||||
$_duplicate=""
|
||||
# start with no values
|
||||
$_all_values=""
|
||||
# iterate over existing values in the variable
|
||||
if ($_values) {
|
||||
$_values.Split(":") | ForEach {
|
||||
# not an empty string
|
||||
if ($_) {
|
||||
# not a duplicate of _value
|
||||
if ($_ -eq $_value) {
|
||||
$_duplicate="1"
|
||||
}
|
||||
if ($_all_values) {
|
||||
$_all_values="${_all_values}:$_"
|
||||
} else {
|
||||
$_all_values="$_"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
# append only non-duplicates
|
||||
if (!$_duplicate) {
|
||||
# avoid leading separator
|
||||
if ($_all_values) {
|
||||
$_all_values="${_all_values}:${_value}"
|
||||
} else {
|
||||
$_all_values="${_value}"
|
||||
}
|
||||
}
|
||||
|
||||
# export the updated variable
|
||||
Set-Item env:\$_listname -Value "$_all_values"
|
||||
}
|
||||
|
||||
# function to prepend a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as trailing separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
function colcon_prepend_unique_value {
|
||||
param (
|
||||
$_listname,
|
||||
$_value
|
||||
)
|
||||
|
||||
# get values from variable
|
||||
if (Test-Path Env:$_listname) {
|
||||
$_values=(Get-Item env:$_listname).Value
|
||||
} else {
|
||||
$_values=""
|
||||
}
|
||||
# start with the new value
|
||||
$_all_values="$_value"
|
||||
# iterate over existing values in the variable
|
||||
if ($_values) {
|
||||
$_values.Split(":") | ForEach {
|
||||
# not an empty string
|
||||
if ($_) {
|
||||
# not a duplicate of _value
|
||||
if ($_ -ne $_value) {
|
||||
# keep non-duplicate values
|
||||
$_all_values="${_all_values}:$_"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
# export the updated variable
|
||||
Set-Item env:\$_listname -Value "$_all_values"
|
||||
}
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
function colcon_package_source_powershell_script {
|
||||
param (
|
||||
$_colcon_package_source_powershell_script
|
||||
)
|
||||
# source script with conditional trace output
|
||||
if (Test-Path $_colcon_package_source_powershell_script) {
|
||||
if ($env:COLCON_TRACE) {
|
||||
echo ". '$_colcon_package_source_powershell_script'"
|
||||
}
|
||||
. "$_colcon_package_source_powershell_script"
|
||||
} else {
|
||||
Write-Error "not found: '$_colcon_package_source_powershell_script'"
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
# a powershell script is able to determine its own path
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
|
||||
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/pythonpath.ps1"
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/ament_prefix_path.ps1"
|
||||
|
||||
Remove-Item Env:\COLCON_CURRENT_PREFIX
|
||||
@@ -0,0 +1,87 @@
|
||||
# generated from colcon_core/shell/template/package.sh.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# function to prepend a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as trailing separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
_colcon_prepend_unique_value() {
|
||||
# arguments
|
||||
_listname="$1"
|
||||
_value="$2"
|
||||
|
||||
# get values from variable
|
||||
eval _values=\"\$$_listname\"
|
||||
# backup the field separator
|
||||
_colcon_prepend_unique_value_IFS=$IFS
|
||||
IFS=":"
|
||||
# start with the new value
|
||||
_all_values="$_value"
|
||||
# workaround SH_WORD_SPLIT not being set in zsh
|
||||
if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
|
||||
colcon_zsh_convert_to_array _values
|
||||
fi
|
||||
# iterate over existing values in the variable
|
||||
for _item in $_values; do
|
||||
# ignore empty strings
|
||||
if [ -z "$_item" ]; then
|
||||
continue
|
||||
fi
|
||||
# ignore duplicates of _value
|
||||
if [ "$_item" = "$_value" ]; then
|
||||
continue
|
||||
fi
|
||||
# keep non-duplicate values
|
||||
_all_values="$_all_values:$_item"
|
||||
done
|
||||
unset _item
|
||||
# restore the field separator
|
||||
IFS=$_colcon_prepend_unique_value_IFS
|
||||
unset _colcon_prepend_unique_value_IFS
|
||||
# export the updated variable
|
||||
eval export $_listname=\"$_all_values\"
|
||||
unset _all_values
|
||||
unset _values
|
||||
|
||||
unset _value
|
||||
unset _listname
|
||||
}
|
||||
|
||||
# since a plain shell script can't determine its own path when being sourced
|
||||
# either use the provided COLCON_CURRENT_PREFIX
|
||||
# or fall back to the build time prefix (if it exists)
|
||||
_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/cc/ros2/install_sim/arm_sim_gazebo"
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
|
||||
echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
|
||||
unset _colcon_package_sh_COLCON_CURRENT_PREFIX
|
||||
return 1
|
||||
fi
|
||||
COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
unset _colcon_package_sh_COLCON_CURRENT_PREFIX
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_sh_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# source sh hooks
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/pythonpath.sh"
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/ament_prefix_path.sh"
|
||||
|
||||
unset _colcon_package_sh_source_script
|
||||
unset COLCON_CURRENT_PREFIX
|
||||
|
||||
# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
|
||||
@@ -0,0 +1,53 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>arm_sim_gazebo</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Gazebo 仿真后端。兑现 devices/sim/gazebo/manifest.yaml 里声明的
|
||||
package: arm_sim_gazebo / executable: gazebo_sim_launcher。
|
||||
|
||||
职责：
|
||||
1. 把 devices/sim/gazebo/urdf 下的仿真描述展开成 URDF，
|
||||
并归一化成 Gazebo 能接受的形态（见 urdf_normalize.py 的长注释）
|
||||
2. 起 Gazebo、spawn 机械臂、拉控制器、拉 ros_gz 桥
|
||||
3. 把应用层需要的接口对齐成与真机/mock 完全一致
|
||||
（控制器名 rizon_arm_controller、服务与话题名不变），
|
||||
这样应用层代码不需要知道后端是谁
|
||||
|
||||
与真机环境严格隔离：本包只在 ros_gz_sim 环境构建（scripts/build_sim.sh），
|
||||
真机环境不装它。反过来它也不依赖任何真机专属包。
|
||||
</description>
|
||||
<maintainer email="dev@example.com">arm_dev</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>ament_index_python</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
<!-- Gazebo 侧（仅仿真环境有） -->
|
||||
<exec_depend>ros_gz_sim</exec_depend>
|
||||
<exec_depend>ros_gz_bridge</exec_depend>
|
||||
<!-- 触觉适配器订阅 ros_gz_interfaces/msg/Contacts（接触传感器的原始消息），
|
||||
发布 arm_msgs/msg/TactileStatus。两者都是运行期硬依赖。 -->
|
||||
<exec_depend>ros_gz_interfaces</exec_depend>
|
||||
<exec_depend>arm_msgs</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>gz_ros2_control</exec_depend>
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
<exec_depend>joint_trajectory_controller</exec_depend>
|
||||
<!-- 描述与参数 -->
|
||||
<exec_depend>flexiv_description</exec_depend>
|
||||
<exec_depend>arm_control</exec_depend>
|
||||
<exec_depend>arm_application</exec_depend>
|
||||
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
<test_depend>python3-yaml</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,42 @@
|
||||
# generated from colcon_zsh/shell/template/package.zsh.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# a zsh script is able to determine its own path if necessary
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
_colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
|
||||
else
|
||||
_colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_zsh_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# function to convert array-like strings into arrays
|
||||
# to workaround SH_WORD_SPLIT not being set
|
||||
colcon_zsh_convert_to_array() {
|
||||
local _listname=$1
|
||||
local _dollar="$"
|
||||
local _split="{="
|
||||
local _to_array="(\"$_dollar$_split$_listname}\")"
|
||||
eval $_listname=$_to_array
|
||||
}
|
||||
|
||||
# source sh script of this package
|
||||
_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/package.sh"
|
||||
unset convert_zsh_to_array
|
||||
|
||||
unset _colcon_package_zsh_source_script
|
||||
unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
|
||||
@@ -0,0 +1 @@
|
||||
ament_index_python:arm_application:arm_control:arm_msgs:controller_manager:flexiv_description:gz_ros2_control:joint_state_broadcaster:joint_trajectory_controller:launch:launch_ros:rclpy:robot_state_publisher:ros_gz_bridge:ros_gz_interfaces:ros_gz_sim:tf2_ros:xacro
|
||||
Reference in New Issue
Block a user