diff --git a/.devcontainer/dev/Dockerfile b/.devcontainer/dev/Dockerfile index f3f1e91f7..5cbe274b5 100644 --- a/.devcontainer/dev/Dockerfile +++ b/.devcontainer/dev/Dockerfile @@ -37,11 +37,21 @@ RUN apt update && apt install -y --no-install-recommends \ && apt clean # Install custom msgs: +RUN apt update && apt install -y --no-install-recommends \ + ros-$ROS_DISTRO-ewellix-interfaces \ + && rm -rf /var/lib/apt/lists/* \ + && apt autoremove -y \ + && apt clean RUN git clone --depth=1 --filter=blob:none -b v3.1.1 \ https://github.com/frankarobotics/franka_ros2.git src/franka_ros2 \ && cd src/franka_ros2 \ && git sparse-checkout set franka_msgs +# Install debug tools: +RUN apt update && apt install -y --no-install-recommends \ + ros-$ROS_DISTRO-plotjuggler-ros \ + ros-$ROS_DISTRO-rqt-tf-tree + # Setup configuration files: COPY .devcontainer/dev/.dev_bashrc /.dev_bashrc RUN echo "source /.dev_bashrc" >> /home/$REMOTE_USER/.bashrc diff --git a/.devcontainer/dev/devcontainer.json b/.devcontainer/dev/devcontainer.json index 9a01184cd..bdcd92a97 100644 --- a/.devcontainer/dev/devcontainer.json +++ b/.devcontainer/dev/devcontainer.json @@ -14,8 +14,22 @@ "workspaceMount": "source=${localWorkspaceFolder},target=/alliander_robotics,type=bind", "workspaceFolder": "/alliander_robotics", "overrideCommand": true, + "mounts": [ + "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached", + "source=/dev,target=/dev,type=bind,consistency=cached" + ], "runArgs": [ - "--rm" + "--rm", + "--privileged", + "--network=host", + "-e", + "DISPLAY", + "-e", + "NVIDIA_VISIBLE_DEVICES=all", + "-e", + "NVIDIA_DRIVER_CAPABILITIES=all", + "-e", + "RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" ], "customizations": { "vscode": { diff --git a/alliander_core/src/alliander_description/CMakeLists.txt b/alliander_core/src/alliander_description/CMakeLists.txt index bc5167f99..72f19d4f8 100644 --- a/alliander_core/src/alliander_description/CMakeLists.txt +++ b/alliander_core/src/alliander_description/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(ament_cmake_python REQUIRED) # Shared folders: install( - DIRECTORY franka husarion ouster velodyne realsense zed nmea_gps + DIRECTORY franka husarion ouster velodyne realsense zed nmea_gps ewellix DESTINATION share/${PROJECT_NAME} ) diff --git a/alliander_core/src/alliander_description/ewellix/config/controllers.yaml b/alliander_core/src/alliander_description/ewellix/config/controllers.yaml new file mode 100644 index 000000000..0b1067b62 --- /dev/null +++ b/alliander_core/src/alliander_description/ewellix/config/controllers.yaml @@ -0,0 +1,23 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +# Based on: +# https://github.com/clearpathrobotics/ewellix_lift_common/blob/0.2.1/ewellix_description/config/control/jpc.yaml + +/**: + controller_manager: + ros__parameters: + update_rate: 10 # Hz + handle_exceptions: false + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + lift_position_controller: + type: position_controllers/JointGroupPositionController + + lift_position_controller: + ros__parameters: + joints: + - lift_lower_joint diff --git a/alliander_core/src/alliander_description/ewellix/urdf/adapted/ewellix_macro.xacro b/alliander_core/src/alliander_description/ewellix/urdf/adapted/ewellix_macro.xacro new file mode 100644 index 000000000..affb7d0cb --- /dev/null +++ b/alliander_core/src/alliander_description/ewellix/urdf/adapted/ewellix_macro.xacro @@ -0,0 +1,290 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + mock_components/GenericSystem + 0.0 + true + + + ewellix_driver/EwellixHardwareInterface + ${port} + ${baud} + ${timeout} + ${conversion} + ${rated_effort} + ${tolerance} + ${parameters.encoder_limits.lower} + ${parameters.encoder_limits.upper} + + + + + + 0.001 + + + + + + + ${tf_prefix}lower_joint + 1 + + + + + + + \ No newline at end of file diff --git a/alliander_core/src/alliander_description/ewellix/urdf/adapted/ewellix_macro.xacro.license b/alliander_core/src/alliander_description/ewellix/urdf/adapted/ewellix_macro.xacro.license new file mode 100644 index 000000000..b4d18b041 --- /dev/null +++ b/alliander_core/src/alliander_description/ewellix/urdf/adapted/ewellix_macro.xacro.license @@ -0,0 +1,3 @@ +SPDX-FileCopyrightText: Alliander N. V. + +SPDX-License-Identifier: Apache-2.0 \ No newline at end of file diff --git a/alliander_core/src/alliander_description/ewellix/urdf/ewellix.urdf.xacro b/alliander_core/src/alliander_description/ewellix/urdf/ewellix.urdf.xacro new file mode 100644 index 000000000..9baabe213 --- /dev/null +++ b/alliander_core/src/alliander_description/ewellix/urdf/ewellix.urdf.xacro @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find alliander_description)/ewellix/config/controllers.yaml + + $(arg namespace) + + + + + + + + + + + + + + + + + + + + + true + + + \ No newline at end of file diff --git a/alliander_core/src/alliander_description/ewellix/urdf/original/ewellix_macro.xacro b/alliander_core/src/alliander_description/ewellix/urdf/original/ewellix_macro.xacro new file mode 100644 index 000000000..a5b7aff48 --- /dev/null +++ b/alliander_core/src/alliander_description/ewellix/urdf/original/ewellix_macro.xacro @@ -0,0 +1,279 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + mock_components/GenericSystem + 0.0 + true + + + ewellix_driver/EwellixHardwareInterface + ${port} + ${baud} + ${timeout} + ${conversion} + ${rated_effort} + ${tolerance} + ${parameters.encoder_limits.lower} + ${parameters.encoder_limits.upper} + + + + + + ${parameters.lower.joint.limit.lower} + + + + + + + ${tf_prefix}lower_joint + 1 + + + + + + + diff --git a/alliander_core/src/alliander_description/ewellix/urdf/original/ewellix_macro.xacro.license b/alliander_core/src/alliander_description/ewellix/urdf/original/ewellix_macro.xacro.license new file mode 100644 index 000000000..b4d18b041 --- /dev/null +++ b/alliander_core/src/alliander_description/ewellix/urdf/original/ewellix_macro.xacro.license @@ -0,0 +1,3 @@ +SPDX-FileCopyrightText: Alliander N. V. + +SPDX-License-Identifier: Apache-2.0 \ No newline at end of file diff --git a/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py b/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py index 22f4888a7..cc6cc2571 100644 --- a/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py +++ b/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py @@ -191,6 +191,8 @@ def default_link_to_child(self) -> str: return "base_link" case "franka": return "fr3_hand" + case "ewellix": + return "lift_mount" case _: raise ValueError( f"No link_to_child for unknown platform name: {self.name}" @@ -331,6 +333,17 @@ class GPS(Platform): diagnostic_timeouts: tuple[int, int, int] = (3, 5, 10) +@dataclass +class Lift(Platform): + """Configuration for a Lift platform. + + Attributes: + platform_type (str): Type identifier for the platform. + """ + + platform_type: str = "Lift" + + # Configurations containing lists of platforms: @dataclass class PlatformList(Config): @@ -342,7 +355,7 @@ class PlatformList(Config): platforms: List[ Annotated[ - Union[Platform, Arm, Vehicle, Lidar, Camera, GPS], + Union[Platform, Arm, Vehicle, Lidar, Camera, GPS, Lift], Discriminator(field="platform_type", include_supertypes=True), ] ] = field(default_factory=list) diff --git a/alliander_ewellix/alliander_ewellix.Dockerfile b/alliander_ewellix/alliander_ewellix.Dockerfile new file mode 100644 index 000000000..c288773b7 --- /dev/null +++ b/alliander_ewellix/alliander_ewellix.Dockerfile @@ -0,0 +1,36 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 +ARG BASE_IMAGE=ubuntu:latest +FROM $BASE_IMAGE + +ARG COLCON_BUILD_SEQUENTIAL +ENV ROS_DISTRO=jazzy + +# Install Ewellix packages: +WORKDIR /$WORKDIR/external +RUN apt update \ + && apt install -y ros-$ROS_DISTRO-ewellix-description \ + && git clone -b 0.2.3 https://github.com/clearpathrobotics/ewellix_lift.git src/ewellix_lift \ + && git clone https://github.com/joshnewans/serial.git src/serial \ + && rosdep update --rosdistro $ROS_DISTRO \ + && rosdep install --from-paths src -y -i +RUN /$WORKDIR/colcon_build.sh + +# Install repo packages: +WORKDIR /$WORKDIR/ros +COPY alliander_core/src/ /$WORKDIR/ros/src +COPY alliander_ewellix/src/ /$WORKDIR/ros/src +RUN /$WORKDIR/colcon_build.sh + +# Install python dependencies: +WORKDIR $WORKDIR +COPY pyproject.toml/ /$WORKDIR/pyproject.toml +RUN uv sync --group alliander-gps \ + && echo "export PYTHONPATH=\"$(dirname $(dirname $(uv python find)))/lib/python3.12/site-packages:\$PYTHONPATH\"" >> /root/.bashrc \ + && echo "export PATH=\"$(dirname $(dirname $(uv python find)))/bin:\$PATH\"" >> /root/.bashrc + +# Finalize +WORKDIR /$WORKDIR +ENTRYPOINT ["/entrypoint.sh"] +CMD ["sleep", "infinity"] diff --git a/alliander_ewellix/docker-compose.yml b/alliander_ewellix/docker-compose.yml new file mode 100644 index 000000000..769dcf556 --- /dev/null +++ b/alliander_ewellix/docker-compose.yml @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 +services: + alliander_ewellix: + image: allianderrobotics/ewellix + container_name: alliander_ewellix + network_mode: host + privileged: true + mem_limit: 6gb + tty: true + volumes: + - "/tmp/.X11-unix:/tmp/.X11-unix" + - "/dev:/dev" + env_file: + - .env + command: ["/bin/bash", "-c", "ros2 launch alliander_ewellix ewellix.launch.py"] diff --git a/alliander_ewellix/src/alliander_ewellix/CMakeLists.txt b/alliander_ewellix/src/alliander_ewellix/CMakeLists.txt new file mode 100644 index 000000000..f5352407e --- /dev/null +++ b/alliander_ewellix/src/alliander_ewellix/CMakeLists.txt @@ -0,0 +1,24 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.5) +project(alliander_ewellix) + +# CMake dependencies: +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +# Shared folders: +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +# Default test: +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/alliander_ewellix/src/alliander_ewellix/launch/controllers.launch.py b/alliander_ewellix/src/alliander_ewellix/launch/controllers.launch.py new file mode 100644 index 000000000..da54db4ad --- /dev/null +++ b/alliander_ewellix/src/alliander_ewellix/launch/controllers.launch.py @@ -0,0 +1,67 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +from alliander_utilities.config_objects import Lift +from alliander_utilities.launch_argument import LaunchArgument +from alliander_utilities.register import Register +from launch import LaunchContext, LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +TIMEOUT = 100 + +platform_arg = LaunchArgument("platform_config", "") + + +def launch_setup(context: LaunchContext) -> list: + """Setup the launch description for the Franka controllers. + + Args: + context (LaunchContext): The launch context. + + Returns: + list: A list of actions to be executed in the launch description. + """ + lift_config = Lift.from_str(platform_arg.string_value(context)) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--switch-timeout", + str(TIMEOUT), + ], + namespace=lift_config.namespace, + ) + + position_controller = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "lift_position_controller", + "--switch-timeout", + str(TIMEOUT), + ], + namespace=lift_config.namespace, + ) + + return [ + Register.on_exit(joint_state_broadcaster_spawner, context), + Register.on_exit(position_controller, context), + ] + + +def generate_launch_description() -> LaunchDescription: + """Generate the launch description for the Ewellix lift controllers. + + Returns: + LaunchDescription: The launch description containing the nodes and actions. + """ + return LaunchDescription( + [ + platform_arg.declaration, + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/alliander_ewellix/src/alliander_ewellix/launch/ewellix.launch.py b/alliander_ewellix/src/alliander_ewellix/launch/ewellix.launch.py new file mode 100644 index 000000000..5890b9328 --- /dev/null +++ b/alliander_ewellix/src/alliander_ewellix/launch/ewellix.launch.py @@ -0,0 +1,74 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 +from alliander_utilities.config_objects import Lift +from alliander_utilities.launch_argument import LaunchArgument +from alliander_utilities.launch_utils import SKIP, state_publisher_node +from alliander_utilities.register import Register, RegisteredLaunchDescription +from alliander_utilities.ros_utils import get_file_path +from launch import LaunchContext, LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node, SetParameter + +platform_arg = LaunchArgument("platform_config", "") + + +def launch_setup(context: LaunchContext) -> list: + """The launch setup. + + Args: + context (LaunchContext): The launch context. + + Returns: + list: The actions to start. + """ + lift_config = Lift.from_str(platform_arg.string_value(context)) + + state_publisher = state_publisher_node( + namespace=lift_config.namespace, + platform="ewellix", + xacro="ewellix.urdf.xacro", + xacro_arguments={ + "namespace": lift_config.namespace, + "sim_gazebo": str(lift_config.simulation), + "parent": "" if lift_config.parent.link else "world", + "childs": str( + [ + [child.connects_to, child.namespace, child.link] + for child in lift_config.childs + ] + ), + }, + ) + + controllers = RegisteredLaunchDescription( + get_file_path("alliander_ewellix", ["launch"], "controllers.launch.py"), + launch_arguments={"platform_config": lift_config.to_str()}, + ) + + driver = Node( + package="ewellix_driver", + executable="ewellix_node", + namespace=lift_config.namespace, + ) + + return [ + SetParameter(name="use_sim_time", value=lift_config.simulation), + Register.on_start(state_publisher, context), + Register.group(controllers, context), + Register.on_start(driver, context) if not lift_config.simulation else SKIP, + ] + + +def generate_launch_description() -> LaunchDescription: + """Generate the launch description. + + Returns: + LaunchDescription: The launch description. + """ + return LaunchDescription( + [ + platform_arg.declaration, + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/alliander_ewellix/src/alliander_ewellix/package.xml b/alliander_ewellix/src/alliander_ewellix/package.xml new file mode 100644 index 000000000..d4924d0a8 --- /dev/null +++ b/alliander_ewellix/src/alliander_ewellix/package.xml @@ -0,0 +1,28 @@ + + + + + + + alliander_ewellix + 0.1.0 + Contains the Alliander Robotics software for the Ewellix lift. + Alliander Robotics + Apache 2.0 + + ament_cmake + ament_cmake_python + + rclcpp + rclcpp_action + + ament_lint_auto + + + ament_cmake + + diff --git a/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py b/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py index ab3641c92..880cf106b 100644 --- a/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py +++ b/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py @@ -51,6 +51,8 @@ def __init__(self, config: VisualizationConfig, platform_list: PlatformList): self.add_depth_camera(Camera.from_str(platform.to_str())) case "GPS": self.add_gps(GPS.from_str(platform.to_str())) + case "Lift": + pass case _: raise NotImplementedError( f"Configuration for platform {type(platform).__name__} is not implemented." diff --git a/common/get_vendor_descriptions.sh b/common/get_vendor_descriptions.sh index 1b9fee8a2..859f446de 100755 --- a/common/get_vendor_descriptions.sh +++ b/common/get_vendor_descriptions.sh @@ -25,4 +25,5 @@ cd ../.. apt update && apt install -y --no-install-recommends \ ros-$ROS_DISTRO-husarion-components-description \ ros-$ROS_DISTRO-velodyne-description \ - ros-$ROS_DISTRO-zed-msgs + ros-$ROS_DISTRO-zed-msgs \ + ros-$ROS_DISTRO-ewellix-description diff --git a/components.yml b/components.yml index c3a009c3b..e36c0266c 100644 --- a/components.yml +++ b/components.yml @@ -18,6 +18,10 @@ diagnostics: base_image: allianderrobotics/base repository: allianderrobotics/diagnostics dockerfile: alliander_diagnostics/alliander_diagnostics.Dockerfile +ewellix: + base_image: allianderrobotics/base + repository: allianderrobotics/ewellix + dockerfile: alliander_ewellix/alliander_ewellix.Dockerfile franka: base_image: allianderrobotics/base repository: allianderrobotics/franka diff --git a/conftest.py b/conftest.py index 7714ad560..8b8bc96c7 100644 --- a/conftest.py +++ b/conftest.py @@ -16,7 +16,6 @@ from _pytest.config import Config from _pytest.config.argparsing import Parser from _pytest.fixtures import SubRequest -from alliander_utilities.config_objects import PlatformList, SimulatorConfig from rclpy.node import Node from termcolor import cprint @@ -158,14 +157,14 @@ def create_compose_file(request: SubRequest) -> list: compose.predefined_configuration = PredefinedConfigurations() compose.visualization = False - platform_list = PlatformList() - platform_list.platforms = request.cls.platforms.values() - compose.predefined_configuration.plat_conf = platform_list + compose.predefined_configuration.plat_conf.platforms = ( + request.cls.platforms.values() + ) load_ui = os.getenv("GAZEBO_UI", default="false").lower() == "true" world = getattr(request.cls, "world", "empty.sdf") - sim_config = SimulatorConfig(load_ui=load_ui, world=world) - compose.predefined_configuration.sim_conf = sim_config + compose.predefined_configuration.sim_conf.load_ui = load_ui + compose.predefined_configuration.sim_conf.world = world # Propagate dev mounts dev_mounts = os.getenv("DEV_MOUNTS", default="false") == "true" diff --git a/predefined_configurations.py b/predefined_configurations.py index 63e8aaa0d..149dfa3ef 100644 --- a/predefined_configurations.py +++ b/predefined_configurations.py @@ -13,6 +13,7 @@ Arm, Camera, Lidar, + Lift, Platform, PlatformList, SimulatorConfig, @@ -108,6 +109,27 @@ def config_realsense(self) -> None: # noqa: D102 def config_zed(self) -> None: # noqa: D102 self.plat_conf.platforms = [Camera("zed", (0, 0, 0.5), namespace="zed")] + # Ewellix: + @register_configuration("ewellix") + def config_ewellix(self) -> None: # noqa: D102 + self.plat_conf.platforms = [Lift("ewellix")] + + @register_configuration("ewellix_velodyne") + def config_ewellix_velodyne(self) -> None: # noqa: D102 + lift = Lift("ewellix") + lidar = Lidar("velodyne") + + link(lift, lidar) + self.plat_conf.platforms = [lift, lidar] + + @register_configuration("ewellix_franka") + def config_ewellix_franka(self) -> None: # noqa: D102 + lift = Lift("ewellix") + arm = Arm("franka") + + link(lift, arm) + self.plat_conf.platforms = [lift, arm] + # Franka: @register_configuration("franka") def config_franka(self) -> None: # noqa: D102 diff --git a/start.py b/start.py index 9e4c2b8d3..871b71133 100755 --- a/start.py +++ b/start.py @@ -54,6 +54,7 @@ class Compose: "${HOME}/.nix-profile/bin/nvim:/usr/bin/nvim", "/nix/store:/nix/store", "./pyproject.toml:/alliander/pyproject.toml", + "./alliander_core/src/alliander_description:/alliander/ros/src/alliander_description", "./alliander_core/src/alliander_utilities:/alliander/ros/src/alliander_utilities", ], }