Переработана вся структура запуска файлов. Теперь в iiwa_bringup хранятся только launch файлы.
Все конфигурационные файлы переехали в пакет `iiwa_config`. также там появился общий конфигурационный файл, который упращает запуск `digital_twin.launch.py` Добавлен дополнительный пакет `iiwa_utils`, в который переехал весь код с папки `iiwa_bringup/utils`, а также все тестовые ноды переехали с `iiwa_object_spawner` в этот пакет Пакеты `iiwa_bringup` и `iiwa_description` переведены на `ament_cmake` для дальнейшего удобсва - пакеты не будут содержать код для реализации
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
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
|
||||
|
||||
http://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.
|
||||
@@ -0,0 +1,25 @@
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional, Union
|
||||
|
||||
import xacro
|
||||
|
||||
|
||||
def load_robot_description(
|
||||
model_path: Union[str, Path],
|
||||
robot_name: str,
|
||||
xacro_args: Optional[Dict[str, str]] = None,
|
||||
) -> str:
|
||||
model_path = Path(model_path)
|
||||
suffix = model_path.suffix.lower()
|
||||
|
||||
if suffix == ".xacro":
|
||||
mappings = {"name": str(robot_name)}
|
||||
if xacro_args:
|
||||
mappings.update({k: str(v) for k, v in xacro_args.items()})
|
||||
|
||||
return xacro.process_file(str(model_path), mappings=mappings).toxml()
|
||||
|
||||
if suffix == ".urdf":
|
||||
return model_path.read_text(encoding="utf-8")
|
||||
|
||||
raise FileNotFoundError(f"Supported file formats: .xacro/.urdf, got: {model_path}")
|
||||
@@ -0,0 +1,87 @@
|
||||
import time
|
||||
|
||||
# generic ros libraries
|
||||
import rclpy
|
||||
from moveit.core.robot_state import RobotState
|
||||
from moveit.planning import MoveItPy
|
||||
from rclpy.impl.rcutils_logger import RcutilsLogger
|
||||
from rclpy.logging import get_logger
|
||||
|
||||
|
||||
def plan_and_execute(robot: MoveItPy,
|
||||
planning_component,
|
||||
logger: RcutilsLogger,
|
||||
single_plan_parameters=None,
|
||||
multi_plan_parameters=None,
|
||||
sleep_time=0.0):
|
||||
logger.info("Planning trajectory")
|
||||
|
||||
if multi_plan_parameters is not None:
|
||||
plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters)
|
||||
elif single_plan_parameters is not None:
|
||||
plan_result = planning_component.plan(
|
||||
single_plan_parameters=single_plan_parameters
|
||||
)
|
||||
else:
|
||||
plan_result = planning_component.plan()
|
||||
|
||||
# execute the plan
|
||||
if plan_result:
|
||||
logger.info("Executing plan")
|
||||
robot_trajectory = plan_result.trajectory
|
||||
robot.execute(robot_trajectory, controllers=[])
|
||||
else:
|
||||
logger.error("Planning failed")
|
||||
|
||||
time.sleep(sleep_time)
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
logger = get_logger("moveit_py.pose_goal")
|
||||
|
||||
iiwa = MoveItPy(node_name="moveit_py")
|
||||
iiwa_arm = iiwa.get_planning_component("iiwa_arm")
|
||||
logger.info("MoveItPy instance created")
|
||||
|
||||
iiwa_arm.set_start_state(configuration_name="ready")
|
||||
iiwa_arm.set_goal_state(configuration_name="extended")
|
||||
|
||||
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
|
||||
|
||||
robot_model = iiwa.get_robot_model()
|
||||
robot_state = RobotState(robot_model)
|
||||
|
||||
# randomize the robot state
|
||||
robot_state.set_to_random_positions()
|
||||
|
||||
# set plan start state to current state
|
||||
iiwa_arm.set_start_state_to_current_state()
|
||||
|
||||
# set goal state to the initialized robot state
|
||||
logger.info("Set goal state to the initialized robot state")
|
||||
iiwa_arm.set_goal_state(robot_state=robot_state)
|
||||
|
||||
# plan to goal
|
||||
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
|
||||
|
||||
# set plan start state to current state
|
||||
iiwa_arm.set_start_state_to_current_state()
|
||||
|
||||
# set pose goal with PoseStamped message
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
|
||||
pose_goal = PoseStamped()
|
||||
pose_goal.header.frame_id = "base_link"
|
||||
pose_goal.pose.orientation.w = 1.0
|
||||
pose_goal.pose.position.x = 0.28
|
||||
pose_goal.pose.position.y = -0.2
|
||||
pose_goal.pose.position.z = 0.5
|
||||
iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="link7")
|
||||
|
||||
# plan to goal
|
||||
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,85 @@
|
||||
# /Ros2Supervisor/spawn_node_from_string / webots_ros2_msgs/srv/SpawnNodeFromString
|
||||
# /Ros2Supervisor/spawn_urdf_robot
|
||||
|
||||
|
||||
from random import randint
|
||||
|
||||
import rclpy
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from rclpy.task import Future
|
||||
from webots_ros2_msgs.srv import SpawnNodeFromString
|
||||
|
||||
|
||||
class ObjectSpawner(Node):
|
||||
def __init__(self):
|
||||
super().__init__("object_spawner")
|
||||
|
||||
# параметры передаваемые
|
||||
self.declare_parameter('objects_path', '')
|
||||
|
||||
# переменные
|
||||
self._object_count: int = randint(1, 5)
|
||||
self._spawned_count: int = 0
|
||||
self._call_in_progress: bool = False
|
||||
|
||||
# Реализация
|
||||
self.cli = self.create_client(
|
||||
SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
|
||||
)
|
||||
|
||||
while not self.cli.wait_for_service(timeout_sec=10):
|
||||
self.get_logger().warning(
|
||||
"service /Ros2Supervisor/spawn_node_from_string not available, waiting again..."
|
||||
)
|
||||
|
||||
self._timer = self.create_timer(0.1, self.timer_callback)
|
||||
|
||||
# TODO: переделать процесс спавна, должен сформировать все proto файлы, после чего заспавнить все объекты
|
||||
def timer_callback(self):
|
||||
# TODO: здесь должен получать объекты
|
||||
if self._spawned_count >= self._object_count:
|
||||
self.get_logger().info(f"All {self._object_count} objects spawned")
|
||||
self._timer.cancel()
|
||||
return
|
||||
|
||||
if self._call_in_progress:
|
||||
return
|
||||
|
||||
data = 'Solid { name "test_box2" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }'
|
||||
data = self._create_group(children=[data])
|
||||
|
||||
req = SpawnNodeFromString.Request(data=data, check_fields=True)
|
||||
self.get_logger().info(
|
||||
f"Calling spawn service #{self._spawned_count + 1}/{self._object_count}"
|
||||
)
|
||||
self._call_in_progress = True
|
||||
future = self.cli.call_async(req)
|
||||
future.add_done_callback(self._response_callback)
|
||||
|
||||
def _response_callback(self, future: Future):
|
||||
try:
|
||||
response = future.result()
|
||||
self.get_logger().info(f"Spawn service response: {response}")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Service call field: {e}")
|
||||
finally:
|
||||
self._call_in_progress = False
|
||||
self._spawned_count += 1
|
||||
|
||||
# Вспомогательные методы формирования кода для спавна объектов
|
||||
def _create_group(self, children: list[str]) -> str:
|
||||
return f"DEF OBJECTS Group {{children [{" ".join(children)}]}}"
|
||||
|
||||
|
||||
def main(args=None):
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
node = ObjectSpawner()
|
||||
rclpy.spin(node=node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,226 @@
|
||||
import os
|
||||
from dataclasses import dataclass, fields, is_dataclass
|
||||
from typing import Any, Dict, Type, TypeVar
|
||||
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
T = TypeVar("T")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RobotCfg:
|
||||
name: str
|
||||
ip: str
|
||||
port: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class WebotsCfg:
|
||||
world: str
|
||||
transform: str
|
||||
rotation: str
|
||||
controller_timer: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RvizCfg:
|
||||
config: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DigitalTwinCfg:
|
||||
webots: WebotsCfg
|
||||
rviz: RvizCfg
|
||||
description: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MoveitCfg:
|
||||
srdf: str
|
||||
kinematics: str
|
||||
joint_limits: str
|
||||
pilz_limits: str
|
||||
initial_positions: str
|
||||
moveit_controllers: str
|
||||
moveit_cpp: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ControllerCfg:
|
||||
controller_path: str
|
||||
moveit: MoveitCfg
|
||||
|
||||
|
||||
class SettingsError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Settings:
|
||||
robot: RobotCfg
|
||||
digital_twin: DigitalTwinCfg
|
||||
controller: ControllerCfg
|
||||
|
||||
def to_dict(self) -> Dict[str, Any]:
|
||||
"""Serialize dataclass tree -> dict (recursive)."""
|
||||
|
||||
def _convert(obj: Any) -> Any:
|
||||
if is_dataclass(obj):
|
||||
return {f.name: _convert(getattr(obj, f.name)) for f in fields(obj)}
|
||||
if isinstance(obj, dict):
|
||||
return {k: _convert(v) for k, v in obj.items()}
|
||||
if isinstance(obj, (list, tuple)):
|
||||
return [_convert(x) for x in obj]
|
||||
return obj
|
||||
|
||||
return _convert(self)
|
||||
|
||||
def to_yaml(self) -> str:
|
||||
return yaml.safe_dump(self.to_dict(), sort_keys=False, allow_unicode=True)
|
||||
|
||||
def save_yaml(self, path: str) -> None:
|
||||
path = os.path.abspath(path)
|
||||
os.makedirs(os.path.dirname(path), exist_ok=True)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
f.write(self.to_yaml())
|
||||
|
||||
@classmethod
|
||||
def from_yaml(cls: Type[T], settings_path: str, check_files: bool = True) -> T:
|
||||
return build_settings(settings_path=settings_path, check_files=check_files)
|
||||
|
||||
|
||||
def load_yaml(path: str) -> Dict[str, Any]:
|
||||
path = os.path.abspath(path)
|
||||
if not os.path.exists(path):
|
||||
raise SettingsError(f"settings yaml not found: {path}")
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
if not isinstance(data, dict):
|
||||
raise SettingsError("settings yaml root must be a mapping (dict)")
|
||||
return data
|
||||
|
||||
|
||||
def resolve_path(value: str, settings_dir: str) -> str:
|
||||
"""
|
||||
Supported:
|
||||
- pkg://<pkg>/<rel>
|
||||
- /abs/path
|
||||
- ./relative or relative (relative to settings.yaml dir)
|
||||
Returns absolute normalized path.
|
||||
"""
|
||||
if not isinstance(value, str) or not value.strip():
|
||||
raise SettingsError("empty path value in settings")
|
||||
|
||||
value = value.strip()
|
||||
|
||||
if value.startswith("pkg://"):
|
||||
rest = value[len("pkg://") :]
|
||||
if "/" not in rest:
|
||||
raise SettingsError(f"bad pkg uri: {value} (need pkg://<pkg>/<path>)")
|
||||
pkg, rel = rest.split("/", 1)
|
||||
share = get_package_share_directory(pkg)
|
||||
return os.path.normpath(os.path.join(share, rel))
|
||||
|
||||
if os.path.isabs(value):
|
||||
return os.path.normpath(value)
|
||||
|
||||
return os.path.normpath(os.path.join(settings_dir, value))
|
||||
|
||||
|
||||
def require(d: Dict[str, Any], key: str) -> Any:
|
||||
if key not in d:
|
||||
raise SettingsError(f"missing key '{key}'")
|
||||
return d[key]
|
||||
|
||||
|
||||
def assert_file(path: str, key: str) -> None:
|
||||
if not os.path.exists(path):
|
||||
raise SettingsError(f"file for '{key}' does not exist: {path}")
|
||||
if not os.path.isfile(path):
|
||||
raise SettingsError(f"path for '{key}' is not a file: {path}")
|
||||
|
||||
|
||||
def build_settings(settings_path: str, check_files: bool = True) -> Settings:
|
||||
settings_path = os.path.abspath(settings_path)
|
||||
settings_dir = os.path.dirname(settings_path)
|
||||
|
||||
raw = load_yaml(settings_path)
|
||||
|
||||
# robot
|
||||
robot_raw = require(raw, "robot")
|
||||
robot = RobotCfg(
|
||||
name=str(require(robot_raw, "name")),
|
||||
ip=str(require(robot_raw, "ip")),
|
||||
port=int(require(robot_raw, "port")),
|
||||
)
|
||||
|
||||
# digital_twin
|
||||
dt_raw = require(raw, "digital_twin")
|
||||
webots_raw = require(dt_raw, "webots")
|
||||
rviz_raw = require(dt_raw, "rviz")
|
||||
|
||||
webots = WebotsCfg(
|
||||
world=resolve_path(str(require(webots_raw, "world")), settings_dir),
|
||||
transform=str(require(webots_raw, "transform")),
|
||||
rotation=str(require(webots_raw, "rotation")),
|
||||
controller_timer=int(require(webots_raw, "controller_timer")),
|
||||
)
|
||||
|
||||
rviz = RvizCfg(config=resolve_path(str(require(rviz_raw, "config")), settings_dir))
|
||||
|
||||
digital_twin = DigitalTwinCfg(
|
||||
webots=webots,
|
||||
rviz=rviz,
|
||||
description=resolve_path(str(require(dt_raw, "description")), settings_dir),
|
||||
)
|
||||
|
||||
# controller, moveit
|
||||
ctrl_raw = require(raw, "controller")
|
||||
moveit_raw = require(ctrl_raw, "moveit")
|
||||
|
||||
moveit = MoveitCfg(
|
||||
srdf=resolve_path(str(require(moveit_raw, "srdf")), settings_dir),
|
||||
kinematics=resolve_path(str(require(moveit_raw, "kinematics")), settings_dir),
|
||||
joint_limits=resolve_path(
|
||||
str(require(moveit_raw, "joint_limits")), settings_dir
|
||||
),
|
||||
pilz_limits=resolve_path(str(require(moveit_raw, "pilz_limits")), settings_dir),
|
||||
initial_positions=resolve_path(
|
||||
str(require(moveit_raw, "initial_positions")), settings_dir
|
||||
),
|
||||
moveit_controllers=resolve_path(
|
||||
str(require(moveit_raw, "moveit_controllers")), settings_dir
|
||||
),
|
||||
moveit_cpp=resolve_path(str(require(moveit_raw, "moveit_cpp")), settings_dir),
|
||||
)
|
||||
|
||||
controller = ControllerCfg(
|
||||
controller_path=resolve_path(
|
||||
str(require(ctrl_raw, "controller_path")), settings_dir
|
||||
),
|
||||
moveit=moveit,
|
||||
)
|
||||
|
||||
s = Settings(robot=robot, digital_twin=digital_twin, controller=controller)
|
||||
|
||||
if check_files:
|
||||
assert_file(s.digital_twin.webots.world, "digital_twin.webots.world")
|
||||
assert_file(s.digital_twin.rviz.config, "digital_twin.rviz.config")
|
||||
assert_file(s.digital_twin.description, "digital_twin.description")
|
||||
assert_file(s.controller.controller_path, "controller.controller_path")
|
||||
|
||||
assert_file(s.controller.moveit.srdf, "controller.moveit.srdf")
|
||||
assert_file(s.controller.moveit.kinematics, "controller.moveit.kinematics")
|
||||
assert_file(s.controller.moveit.joint_limits, "controller.moveit.joint_limits")
|
||||
assert_file(s.controller.moveit.pilz_limits, "controller.moveit.pilz_limits")
|
||||
assert_file(
|
||||
s.controller.moveit.initial_positions, "controller.moveit.initial_positions"
|
||||
)
|
||||
assert_file(
|
||||
s.controller.moveit.moveit_controllers,
|
||||
"controller.moveit.moveit_controllers",
|
||||
)
|
||||
assert_file(s.controller.moveit.moveit_cpp, "controller.moveit.moveit_cpp")
|
||||
|
||||
return s
|
||||
@@ -0,0 +1,18 @@
|
||||
<?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>iiwa_utils</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Вспомогательные файлы для работы всей системы</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/iiwa_utils
|
||||
[install]
|
||||
install_scripts=$base/lib/iiwa_utils
|
||||
@@ -0,0 +1,26 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'iiwa_utils'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='daniel',
|
||||
maintainer_email='grabardm@ml-dev.ru',
|
||||
description='Вспомогательные файлы для работы всей системы',
|
||||
license='Apache-2.0',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'object_spawner = iiwa_utils.object_spawner:main',
|
||||
"motion_planning_test = iiwa_utils.motion_planing_test:main"
|
||||
],
|
||||
},
|
||||
)
|
||||
Reference in New Issue
Block a user