Add MiR100 physical robot support and Warehouse 1 Mir100 world

CustomRobots/mir100/launch/mir100_physical.launch.py

@@ -0,0 +1,73 @@
+import os
+import xacro
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch_ros.actions import Node
+
+
+def launch_setup(context):
+    gz_namespace = LaunchConfiguration("namespace")
+    namespace = gz_namespace.perform(context)
+
+    package_dir = get_package_share_directory("custom_robots")
+    xacro_file = os.path.join(package_dir, "models", "mir100", "mir100.urdf.xacro")
+    robot_description_content = xacro.process_file(
+        xacro_file,
+        mappings={"noise_level": "none", "namespace": namespace},
+    ).toxml()
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        namespace=gz_namespace,
+        output="screen",
+        parameters=[
+            {"robot_description": robot_description_content},
+            {"use_sim_time": False},
+        ],
+    )
+
+    # Talks to the robot itself or to its ROS1 drivers running outside the docker, see mode
+    bridge_node = Node(
+        package="mir100_bridge",
+        executable="bridge_node",
+        namespace=gz_namespace,
+        output="screen",
+        parameters=[
+            {
+                "namespace": namespace,
+                "mode": LaunchConfiguration("mode"),
+                "ros1_hostname": LaunchConfiguration("ros1_hostname"),
+                "ros1_port": LaunchConfiguration("ros1_port"),
+            }
+        ],
+    )
+
+    return [robot_state_publisher_node, bridge_node]
+
+
+def generate_launch_description():
+    declared_arguments = [
+        DeclareLaunchArgument("use_sim_time", default_value="false"),
+        # Pose arguments are unused, RAM passes them to every robot launch
+        DeclareLaunchArgument("x", default_value="0"),
+        DeclareLaunchArgument("y", default_value="0"),
+        DeclareLaunchArgument("z", default_value="0"),
+        DeclareLaunchArgument("R", default_value="0"),
+        DeclareLaunchArgument("P", default_value="0"),
+        DeclareLaunchArgument("Y", default_value="0"),
+        DeclareLaunchArgument("entity", default_value="mir100"),
+        DeclareLaunchArgument("namespace", default_value="mir100"),
+        # robot talks straight to the MiR100, driver talks to a mir_driver
+        DeclareLaunchArgument("mode", default_value="robot"),
+        DeclareLaunchArgument("ros1_hostname", default_value=""),
+        DeclareLaunchArgument("ros1_port", default_value="0"),
+    ]
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

CustomRobots/mir100/mir100_mock/Dockerfile

@@ -0,0 +1,21 @@
+FROM ros:noetic-ros-base
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+        git build-essential python3-catkin-tools python3-rosdep \
+        ros-noetic-rosbridge-server \
+        python3-numpy \
+    && rm -rf /var/lib/apt/lists/*
+
+# Same driver the customer builds from source
+RUN mkdir -p /ws/src && git clone --depth 1 -b noetic https://github.com/DFKI-NI/mir_robot.git /ws/src/mir_robot
+RUN apt-get update && cd /ws \
+    && rosdep install --from-paths src/mir_robot/mir_driver src/mir_robot/mir_msgs src/mir_robot/mir_actions \
+        src/mir_robot/sdc21x0 src/mir_robot/mir_description --ignore-src -r -y \
+    && rm -rf /var/lib/apt/lists/*
+RUN cd /ws && . /opt/ros/noetic/setup.sh \
+    && catkin_make -DCATKIN_WHITELIST_PACKAGES="mir_driver;mir_msgs;mir_actions;sdc21x0;mir_description"
+
+COPY fake_mir.py start_fake_mir.sh start_mir_driver.sh /mock/
+RUN chmod +x /mock/fake_mir.py /mock/start_fake_mir.sh /mock/start_mir_driver.sh
+
+ENTRYPOINT ["/ros_entrypoint.sh"]

CustomRobots/mir100/mir100_mock/README.md

@@ -0,0 +1,32 @@
+# MiR100 mock
+
+Fake MiR100 to develop the real robot integration without hardware.
+
+The real MiR runs its own ROS1 and exposes it through rosbridge (websocket, port 9090).
+`mir_driver` (DFKI-NI/mir_robot, ROS1 Noetic) connects there and republishes the robot topics on the user's roscore.
+This mock reproduces the robot side, so the driver runs unmodified.
+
+- `mir_robot`: ROS1 master, rosbridge on 9090 and `fake_mir.py` (odom, f_scan, b_scan, scan, imu_data, tf, robot_state, cmd_vel as TwistStamped).
+- `mir_driver`: the original `mir.launch` from source, host network, so its ROS1 master is on the host like a real setup.
+
+```
+docker compose up -d
+docker exec -it mir_driver bash -lc 'source /ros_entrypoint.sh; rostopic list'
+```
+
+## Using it with the bridge
+
+The bridge in `Industrial/mir100_bridge` talks straight to the robot by default,
+so for the physical world only the fake robot is needed:
+
+```
+docker compose up --build mir_robot
+```
+
+The bridge runs inside the RADI container, so tell it where the fake robot is,
+for example with `MIR100_ROBOT_IP` set to the address of your machine.
+
+To try the `driver` mode instead, start both services and launch the physical
+world with `mode:=driver`.
+
+To use the real robot, skip this mock and connect to the robot's network.

CustomRobots/mir100/mir100_mock/docker-compose.yml

@@ -0,0 +1,23 @@
+services:
+  # Stands in for the MiR100 itself, a ROS1 master with rosbridge on 9090 like the real robot
+  mir_robot:
+    build: .
+    image: mir100-mock
+    container_name: mir_robot
+    command: /mock/start_fake_mir.sh
+    ports:
+      - "9090:9090"
+    healthcheck:
+      test: ["CMD", "python3", "-c", "import socket; socket.create_connection(('localhost', 9090), 1)"]
+      interval: 2s
+      retries: 30
+
+  # The unmodified driver the customer runs on their own machine
+  mir_driver:
+    image: mir100-mock
+    container_name: mir_driver
+    network_mode: host
+    depends_on:
+      mir_robot:
+        condition: service_healthy
+    command: /mock/start_mir_driver.sh

CustomRobots/mir100/mir100_mock/fake_mir.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python3
+import math
+
+import numpy as np
+import rospy
+import tf2_msgs.msg
+from geometry_msgs.msg import TransformStamped, TwistStamped
+from mir_msgs.msg import RobotState
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Imu, LaserScan
+
+# Room walls in the odom frame, robot starts at the origin
+ROOM = (-5.0, 5.0, -4.0, 4.0)
+
+# Sensor mounting in base_footprint, same values as the real MiR100 description
+LASERS = {
+    "f_scan": ("f_laser_link", 0.4288, 0.2358, math.radians(45)),
+    "b_scan": ("b_laser_link", -0.3548, -0.2352, math.radians(-135)),
+}
+IMU_Z = 0.25
+N_SAMPLES = 541
+ANGLE_MIN = -math.radians(135)
+ANGLE_INC = math.radians(0.5)
+RANGE_MIN = 0.05
+RANGE_MAX = 29.0
+
+
+def log(text):
+    print(f"[MiR100 mock] {text}", flush=True)
+
+
+def yaw_to_quat(yaw):
+    return 0.0, 0.0, math.sin(yaw / 2), math.cos(yaw / 2)
+
+
+def transform(parent, child, x, y, z, yaw, stamp):
+    t = TransformStamped()
+    t.header.stamp = stamp
+    t.header.frame_id = parent
+    t.child_frame_id = child
+    t.transform.translation.x = x
+    t.transform.translation.y = y
+    t.transform.translation.z = z
+    qx, qy, qz, qw = yaw_to_quat(yaw)
+    t.transform.rotation.x = qx
+    t.transform.rotation.y = qy
+    t.transform.rotation.z = qz
+    t.transform.rotation.w = qw
+    return t
+
+
+class FakeMir:
+    def __init__(self):
+        self.x = self.y = self.yaw = 0.0
+        self.vx = self.wz = 0.0
+        self.last_cmd = rospy.Time(0)
+
+        self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=10)
+        self.odom_enc_pub = rospy.Publisher("/odom_enc", Odometry, queue_size=10)
+        self.imu_pub = rospy.Publisher("/imu_data", Imu, queue_size=10)
+        self.state_pub = rospy.Publisher("/robot_state", RobotState, queue_size=10)
+        self.tf_pub = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=10)
+        self.tf_static_pub = rospy.Publisher(
+            "/tf_static", tf2_msgs.msg.TFMessage, queue_size=10, latch=True
+        )
+        self.scan_pubs = {
+            name: rospy.Publisher("/" + name, LaserScan, queue_size=10)
+            for name in LASERS
+        }
+        self.scan_pubs["scan"] = rospy.Publisher("/scan", LaserScan, queue_size=10)
+
+        # MiR software 2.7 and newer expects a stamped twist
+        rospy.Subscriber("/cmd_vel", TwistStamped, self.cmd_cb, queue_size=1)
+
+        self.publish_static()
+
+    def cmd_cb(self, msg):
+        v = msg.twist.linear.x
+        w = msg.twist.angular.z
+        # Commands arrive many times per second so only changes are logged
+        if abs(v - self.vx) > 1e-3 or abs(w - self.wz) > 1e-3:
+            log(f"Command received V={v:.2f} m/s W={w:.2f} rad/s")
+            if not v and not w:
+                self.report_pose("Stopped")
+        self.vx = v
+        self.wz = w
+        self.last_cmd = rospy.Time.now()
+
+    def report_pose(self, text):
+        log(f"{text} at x={self.x:.2f} m y={self.y:.2f} m yaw={self.yaw:.2f} rad")
+
+    def publish_static(self):
+        now = rospy.Time.now()
+        transforms = [transform("base_footprint", "base_link", 0, 0, 0, 0, now)]
+        for _, (frame, lx, ly, lyaw) in LASERS.items():
+            transforms.append(transform("base_link", frame, lx, ly, 0.1914, lyaw, now))
+        transforms.append(transform("base_link", "imu_link", 0, 0, IMU_Z, 0, now))
+        self.tf_static_pub.publish(tf2_msgs.msg.TFMessage(transforms))
+
+    def step(self, dt):
+        # Same safety behaviour as a real base, stop when commands stop arriving
+        if (rospy.Time.now() - self.last_cmd).to_sec() > 0.5:
+            if self.vx or self.wz:
+                self.vx = self.wz = 0.0
+                self.report_pose("No commands for 0.5 s, robot stopped")
+        self.x += self.vx * math.cos(self.yaw) * dt
+        self.y += self.vx * math.sin(self.yaw) * dt
+        self.yaw += self.wz * dt
+
+    def publish_odom(self, now):
+        odom = Odometry()
+        odom.header.stamp = now
+        odom.header.frame_id = "odom"
+        odom.child_frame_id = "base_footprint"
+        odom.pose.pose.position.x = self.x
+        odom.pose.pose.position.y = self.y
+        q = yaw_to_quat(self.yaw)
+        odom.pose.pose.orientation.x, odom.pose.pose.orientation.y = q[0], q[1]
+        odom.pose.pose.orientation.z, odom.pose.pose.orientation.w = q[2], q[3]
+        odom.twist.twist.linear.x = self.vx
+        odom.twist.twist.angular.z = self.wz
+        self.odom_pub.publish(odom)
+        self.odom_enc_pub.publish(odom)
+        self.tf_pub.publish(
+            tf2_msgs.msg.TFMessage(
+                [transform("odom", "base_footprint", self.x, self.y, 0, self.yaw, now)]
+            )
+        )
+
+    def publish_imu(self, now):
+        imu = Imu()
+        imu.header.stamp = now
+        imu.header.frame_id = "imu_link"
+        imu.orientation.z, imu.orientation.w = yaw_to_quat(self.yaw)[2:]
+        imu.angular_velocity.z = self.wz
+        imu.linear_acceleration.z = 9.81
+        self.imu_pub.publish(imu)
+
+    def raycast(self, frame_x, frame_y, frame_yaw):
+        # Lidar pose in the odom frame
+        c, s = math.cos(self.yaw), math.sin(self.yaw)
+        ox = self.x + c * frame_x - s * frame_y
+        oy = self.y + s * frame_x + c * frame_y
+        angles = self.yaw + frame_yaw + ANGLE_MIN + ANGLE_INC * np.arange(N_SAMPLES)
+        dx, dy = np.cos(angles), np.sin(angles)
+        xmin, xmax, ymin, ymax = ROOM
+        with np.errstate(divide="ignore", invalid="ignore"):
+            tx = np.where(dx > 0, (xmax - ox) / dx, (xmin - ox) / dx)
+            ty = np.where(dy > 0, (ymax - oy) / dy, (ymin - oy) / dy)
+        ranges = np.minimum(tx, ty)
+        ranges[~np.isfinite(ranges)] = RANGE_MAX
+        return np.clip(ranges, RANGE_MIN, RANGE_MAX)
+
+    def publish_scans(self, now):
+        for name, (frame, lx, ly, lyaw) in LASERS.items():
+            scan = LaserScan()
+            scan.header.stamp = now
+            scan.header.frame_id = frame
+            scan.angle_min = ANGLE_MIN
+            scan.angle_max = ANGLE_MIN + ANGLE_INC * (N_SAMPLES - 1)
+            scan.angle_increment = ANGLE_INC
+            scan.time_increment = 0.0
+            scan.scan_time = 0.1
+            scan.range_min = RANGE_MIN
+            scan.range_max = RANGE_MAX
+            scan.ranges = self.raycast(lx, ly, lyaw).tolist()
+            self.scan_pubs[name].publish(scan)
+            if name == "f_scan":
+                self.scan_pubs["scan"].publish(scan)
+
+    def publish_state(self):
+        state = RobotState()
+        state.robotState = RobotState.ROBOT_STATE_READY
+        state.robotStateString = "Ready"
+        self.state_pub.publish(state)
+
+    def run(self):
+        log("Ready, waiting for commands")
+        rate = rospy.Rate(50)
+        dt = 1.0 / 50
+        tick = 0
+        while not rospy.is_shutdown():
+            now = rospy.Time.now()
+            self.step(dt)
+            self.publish_odom(now)
+            self.publish_imu(now)
+            if tick % 5 == 0:
+                self.publish_scans(now)
+            if tick % 50 == 0:
+                self.publish_state()
+                if self.vx or self.wz:
+                    self.report_pose("Moving")
+            tick += 1
+            rate.sleep()
+
+
+if __name__ == "__main__":
+    rospy.init_node("fake_mir")
+    FakeMir().run()

CustomRobots/mir100/mir100_mock/start_fake_mir.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+source /opt/ros/noetic/setup.bash
+source /ws/devel/setup.bash
+roscore &
+until rostopic list >/dev/null 2>&1; do sleep 0.5; done
+roslaunch rosbridge_server rosbridge_websocket.launch port:=9090 &
+exec python3 /mock/fake_mir.py

CustomRobots/mir100/mir100_mock/start_mir_driver.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+source /opt/ros/noetic/setup.bash
+source /ws/devel/setup.bash
+roscore &
+until rostopic list >/dev/null 2>&1; do sleep 0.5; done
+roslaunch rosbridge_server rosbridge_websocket.launch port:=9091 &
+exec roslaunch mir_driver mir.launch mir_hostname:=localhost disable_map:=true