diff --git a/CustomRobots/mir100/launch/mir100_physical.launch.py b/CustomRobots/mir100/launch/mir100_physical.launch.py new file mode 100644 index 000000000..c4118d0dd --- /dev/null +++ b/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)] + ) diff --git a/CustomRobots/mir100/mir100_mock/Dockerfile b/CustomRobots/mir100/mir100_mock/Dockerfile new file mode 100644 index 000000000..5bf1570f4 --- /dev/null +++ b/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"] diff --git a/CustomRobots/mir100/mir100_mock/README.md b/CustomRobots/mir100/mir100_mock/README.md new file mode 100644 index 000000000..5bae1e025 --- /dev/null +++ b/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. diff --git a/CustomRobots/mir100/mir100_mock/docker-compose.yml b/CustomRobots/mir100/mir100_mock/docker-compose.yml new file mode 100644 index 000000000..88fbb2ede --- /dev/null +++ b/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 diff --git a/CustomRobots/mir100/mir100_mock/fake_mir.py b/CustomRobots/mir100/mir100_mock/fake_mir.py new file mode 100644 index 000000000..ba1897332 --- /dev/null +++ b/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() diff --git a/CustomRobots/mir100/mir100_mock/start_fake_mir.sh b/CustomRobots/mir100/mir100_mock/start_fake_mir.sh new file mode 100755 index 000000000..1dd689a7a --- /dev/null +++ b/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 diff --git a/CustomRobots/mir100/mir100_mock/start_mir_driver.sh b/CustomRobots/mir100/mir100_mock/start_mir_driver.sh new file mode 100755 index 000000000..06d394c3a --- /dev/null +++ b/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 diff --git a/Industrial/mir100_bridge/README.md b/Industrial/mir100_bridge/README.md new file mode 100644 index 000000000..bf47c55bb --- /dev/null +++ b/Industrial/mir100_bridge/README.md @@ -0,0 +1,53 @@ +# mir100_bridge + +ROS2 node that republishes a MiR100 (real or mocked, see `CustomRobots/mir100/mir100_mock`) as the +same topics the simulated MiR100 uses, so the HAL does not see any difference +between sim and real robot. + +It never installs ROS1. It connects as a plain rosbridge websocket client, so +the whole node is pure ROS2/rclpy and runs inside the RoboticsAcademy docker +like any other exercise node. + +Launched through `CustomRobots/mir100/launch/mir100_physical.launch.py`, the launch file used +for scenes of type physical. + +## Modes + +- `robot` (default): talks straight to the MiR100, which already exposes + rosbridge on port 9090. Nothing to launch besides the exercise, the robot + only has to be reachable on the network. Its address defaults to + `192.168.12.20`, the one it has on its own wifi. +- `driver`: talks to a `mir_driver` running outside the docker, with its own + `rosbridge_server` on port 9091. For setups that already have their own + driver. + +## Topics + +| MiR100 or mir_driver | ROS2 (this node) | +| --- | --- | +| `/cmd_vel` | `/mir100/cmd_vel` | +| `/odom` | `/mir100/odom` | +| `/f_scan` | `/mir100/front_laser/scan` | +| `/b_scan` | `/mir100/back_laser/scan` | +| `/imu_data` | `/mir100/imu` | + +In `robot` mode the twist is sent with a header, as the MiR software 2.7 and +newer expects, `mir_driver` does that itself in `driver` mode. + +## Parameters + +- `mode`, `robot` or `driver`, defaults to `robot`. +- `ros1_hostname`, where to connect. In `robot` mode it defaults to the + `MIR100_ROBOT_IP` environment variable, then `192.168.12.20`. In `driver` + mode it is the machine running `mir_driver`, resolved when empty from the + `MIR100_ROS1_HOST` environment variable, then `host.docker.internal`, then + the container's default gateway (the docker host on Linux), and finally + `localhost`. +- `ros1_port`, defaults to `9090` in `robot` mode and `9091` in `driver` mode. +- `namespace`, defaults to `mir100`. + +## Dependency + +Needs `roslibpy`, a plain Python websocket client, not a ROS package. RADI +installs it in `scripts/RADI/Dockerfile.dependencies_humble`. Outside RADI +use `pip install roslibpy`. diff --git a/Industrial/mir100_bridge/mir100_bridge/__init__.py b/Industrial/mir100_bridge/mir100_bridge/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/Industrial/mir100_bridge/mir100_bridge/bridge_node.py b/Industrial/mir100_bridge/mir100_bridge/bridge_node.py new file mode 100644 index 000000000..2624480ca --- /dev/null +++ b/Industrial/mir100_bridge/mir100_bridge/bridge_node.py @@ -0,0 +1,209 @@ +#!/usr/bin/env python3 +"""ROS2 bridge for the MiR100. + +Connects through rosbridge and republishes the robot with the same ROS2 topics +as the simulated one. In robot mode it talks straight to the MiR100, which +already exposes rosbridge. In driver mode it talks to a mir_driver running +outside the docker, with its own rosbridge_server on top. +""" + +import math + +import rclpy +import roslibpy +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from rclpy.node import Node +from sensor_msgs.msg import Imu, LaserScan + +from mir100_bridge.host import resolve_robot_host, resolve_ros1_host + +# Maps each ROS1 laser topic to its ROS2 topic and TF frame, matching what +# mir100_common.urdf.xacro gives the simulated robot's lasers +LASER_TOPICS = { + "f_scan": ("front_laser/scan", "front_laser_link"), + "b_scan": ("back_laser/scan", "back_laser_link"), +} + +# A paused or reset exercise stops sending cmd_vel, but the real robot keeps +# going at the last speed it got, so once commands go quiet for this long we +# stop it ourselves instead of waiting on the robot's own firmware watchdog +COMMAND_TIMEOUT = 0.3 + + +class Mir100Bridge(Node): + def __init__(self): + super().__init__("mir100_bridge") + + self.declare_parameter("mode", "robot") + self.declare_parameter("ros1_hostname", "") + self.declare_parameter("ros1_port", 0) + self.declare_parameter("namespace", "mir100") + + mode = self.get_parameter("mode").value + configured_host = self.get_parameter("ros1_hostname").value + configured_port = self.get_parameter("ros1_port").value + self.namespace = self.get_parameter("namespace").value.strip("/") + + if mode == "robot": + hostname = resolve_robot_host(configured_host) + port = configured_port or 9090 + elif mode == "driver": + hostname = resolve_ros1_host(configured_host) + port = configured_port or 9091 + else: + raise ValueError(f"unknown mode {mode}, use robot or driver") + + # The MiR software 2.7 and newer expects a stamped twist, mir_driver adds + # the stamp in driver mode but here nobody does it for us + self.stamped = mode == "robot" + + self.get_logger().info( + f"connecting to the MiR100 in {mode} mode at {hostname}:{port}..." + ) + self.last_cmd_time = self.get_clock().now() + self.stopped = True + + self.ros1 = roslibpy.Ros(host=hostname, port=port) + self.ros1.on_ready(self.setup_bridge) + self.ros1.on("error", lambda e: self.get_logger().warn(f"rosbridge error: {e}")) + self.ros1.run(timeout=None) # connects in a background thread without blocking + + def ns(self, topic): + return f"/{self.namespace}/{topic}" + + def setup_bridge(self): + self.get_logger().info("connected to ROS1, wiring topics") + + # forward student commands from ROS2 to the ROS1 robot + cmd_vel_type = ( + "geometry_msgs/TwistStamped" if self.stamped else "geometry_msgs/Twist" + ) + self.cmd_vel_ros1 = roslibpy.Topic(self.ros1, "/cmd_vel", cmd_vel_type) + self.create_subscription(Twist, self.ns("cmd_vel"), self.on_cmd_vel, 10) + self.create_timer(0.1, self.check_command_timeout) + + # republish the robot's ROS1 sensor data as ROS2 + self.odom_pub = self.create_publisher(Odometry, self.ns("odom"), 10) + roslibpy.Topic(self.ros1, "/odom", "nav_msgs/Odometry").subscribe(self.on_odom) + + self.imu_pub = self.create_publisher(Imu, self.ns("imu"), 10) + roslibpy.Topic(self.ros1, "/imu_data", "sensor_msgs/Imu").subscribe(self.on_imu) + + self.scan_pubs = {} + self.scan_frames = {} + for ros1_topic, (ros2_topic, frame) in LASER_TOPICS.items(): + self.scan_pubs[ros1_topic] = self.create_publisher( + LaserScan, self.ns(ros2_topic), 10 + ) + self.scan_frames[ros1_topic] = frame + roslibpy.Topic( + self.ros1, "/" + ros1_topic, "sensor_msgs/LaserScan" + ).subscribe(lambda msg, name=ros1_topic: self.on_scan(name, msg)) + + def on_cmd_vel(self, msg: Twist): + self.last_cmd_time = self.get_clock().now() + self.stopped = False + self.send_cmd_vel(msg) + + def check_command_timeout(self): + idle = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1e9 + if not self.stopped and idle > COMMAND_TIMEOUT: + self.get_logger().warn("No cmd_vel for a while, stopping the robot") + self.send_cmd_vel(Twist()) + self.stopped = True + + def send_cmd_vel(self, msg: Twist): + twist = { + "linear": {"x": msg.linear.x, "y": msg.linear.y, "z": msg.linear.z}, + "angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z}, + } + if self.stamped: + now = self.get_clock().now().nanoseconds + twist = { + "header": { + "frame_id": "", + "stamp": {"secs": now // 10**9, "nsecs": now % 10**9}, + }, + "twist": twist, + } + self.cmd_vel_ros1.publish(roslibpy.Message(twist)) + + def on_odom(self, msg: dict): + odom = Odometry() + odom.header.stamp = self.get_clock().now().to_msg() + odom.header.frame_id = self.ns("odom").lstrip("/") + odom.child_frame_id = self.ns("base_footprint").lstrip("/") + pos = msg["pose"]["pose"]["position"] + ori = msg["pose"]["pose"]["orientation"] + odom.pose.pose.position.x = pos["x"] + odom.pose.pose.position.y = pos["y"] + odom.pose.pose.position.z = pos["z"] + odom.pose.pose.orientation.x = ori["x"] + odom.pose.pose.orientation.y = ori["y"] + odom.pose.pose.orientation.z = ori["z"] + odom.pose.pose.orientation.w = ori["w"] + lin = msg["twist"]["twist"]["linear"] + ang = msg["twist"]["twist"]["angular"] + odom.twist.twist.linear.x = lin["x"] + odom.twist.twist.linear.y = lin["y"] + odom.twist.twist.angular.z = ang["z"] + self.odom_pub.publish(odom) + + def on_imu(self, msg: dict): + imu = Imu() + imu.header.stamp = self.get_clock().now().to_msg() + imu.header.frame_id = self.ns("imu_link").lstrip("/") + ori = msg["orientation"] + imu.orientation.x = ori["x"] + imu.orientation.y = ori["y"] + imu.orientation.z = ori["z"] + imu.orientation.w = ori["w"] + av = msg["angular_velocity"] + imu.angular_velocity.x = av["x"] + imu.angular_velocity.y = av["y"] + imu.angular_velocity.z = av["z"] + la = msg["linear_acceleration"] + imu.linear_acceleration.x = la["x"] + imu.linear_acceleration.y = la["y"] + imu.linear_acceleration.z = la["z"] + self.imu_pub.publish(imu) + + def on_scan(self, ros1_topic: str, msg: dict): + scan = LaserScan() + scan.header.stamp = self.get_clock().now().to_msg() + scan.header.frame_id = self.ns(self.scan_frames[ros1_topic]).lstrip("/") + scan.angle_min = msg["angle_min"] + scan.angle_max = msg["angle_max"] + scan.angle_increment = msg["angle_increment"] + scan.time_increment = msg.get("time_increment", 0.0) + scan.scan_time = msg.get("scan_time", 0.0) + scan.range_min = msg["range_min"] + scan.range_max = msg["range_max"] + scan.ranges = [ + r if r is not None and not math.isnan(r) else msg["range_max"] + for r in msg["ranges"] + ] + scan.intensities = list(msg.get("intensities", [])) + self.scan_pubs[ros1_topic].publish(scan) + + def destroy_node(self): + if self.ros1.is_connected: + self.ros1.close() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = Mir100Bridge() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/Industrial/mir100_bridge/mir100_bridge/host.py b/Industrial/mir100_bridge/mir100_bridge/host.py new file mode 100644 index 000000000..c8723a7bf --- /dev/null +++ b/Industrial/mir100_bridge/mir100_bridge/host.py @@ -0,0 +1,44 @@ +import os +import socket + +# Address the MiR100 has on its own wifi +DEFAULT_ROBOT_IP = "192.168.12.20" + + +def resolve_robot_host(configured=""): + """Find the MiR100 itself, it defaults to the address it has on its own wifi.""" + return configured or os.environ.get("MIR100_ROBOT_IP") or DEFAULT_ROBOT_IP + + +def default_gateway(): + """Return the default gateway of the container, which is the docker host on Linux.""" + with open("/proc/net/route") as routes: + for line in list(routes)[1:]: + fields = line.split() + if fields[1] == "00000000": + # The kernel prints the address as little endian hex + return socket.inet_ntoa(bytes.fromhex(fields[2])[::-1]) + return None + + +def resolve_ros1_host(configured=""): + """Find the machine running the student's ROS1 side, since localhost is the container itself.""" + if configured: + return configured + + from_env = os.environ.get("MIR100_ROS1_HOST") + if from_env: + return from_env + + # Docker Desktop provides this name, Linux needs an extra_hosts entry for it + try: + socket.gethostbyname("host.docker.internal") + return "host.docker.internal" + except OSError: + pass + + try: + gateway = default_gateway() + except OSError: + gateway = None + return gateway or "localhost" diff --git a/Industrial/mir100_bridge/package.xml b/Industrial/mir100_bridge/package.xml new file mode 100644 index 000000000..f43980696 --- /dev/null +++ b/Industrial/mir100_bridge/package.xml @@ -0,0 +1,24 @@ + + + + mir100_bridge + 0.0.1 + + ROS2 node that bridges a MiR100 real (or mocked) robot to the topics the + MiR100 exercise's HAL expects. It talks straight to the robot over the + rosbridge websocket protocol, or to a mir_driver running outside the + docker, so no ROS1 install is needed inside this container. + + aquintan + Apache-2.0 + + rclpy + geometry_msgs + nav_msgs + sensor_msgs + + + + ament_python + + diff --git a/Industrial/mir100_bridge/resource/mir100_bridge b/Industrial/mir100_bridge/resource/mir100_bridge new file mode 100644 index 000000000..e69de29bb diff --git a/Industrial/mir100_bridge/setup.cfg b/Industrial/mir100_bridge/setup.cfg new file mode 100644 index 000000000..5189d39a9 --- /dev/null +++ b/Industrial/mir100_bridge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mir100_bridge +[install] +install_scripts=$base/lib/mir100_bridge diff --git a/Industrial/mir100_bridge/setup.py b/Industrial/mir100_bridge/setup.py new file mode 100644 index 000000000..a8abff5bc --- /dev/null +++ b/Industrial/mir100_bridge/setup.py @@ -0,0 +1,24 @@ +from setuptools import setup + +package_name = "mir100_bridge" + +setup( + name=package_name, + version="0.0.1", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools", "roslibpy"], + zip_safe=True, + maintainer="aquintan", + maintainer_email="aquintana.camacho@gmail.com", + description="ROS1 to ROS2 bridge node for the real/mocked MiR100", + license="Apache-2.0", + entry_points={ + "console_scripts": [ + "bridge_node = mir100_bridge.bridge_node:main", + ], + }, +) diff --git a/Launchers/physical.launch.py b/Launchers/physical.launch.py new file mode 100644 index 000000000..d5e9482f7 --- /dev/null +++ b/Launchers/physical.launch.py @@ -0,0 +1,6 @@ +from launch import LaunchDescription + + +def generate_launch_description(): + # Physical worlds have no simulated scene, the robot launch brings up the drivers + return LaunchDescription() diff --git a/Scenes/warehouse1.world b/Scenes/warehouse1.world index a3d53d44d..606831189 100644 --- a/Scenes/warehouse1.world +++ b/Scenes/warehouse1.world @@ -11,6 +11,9 @@ + + ogre2 + diff --git a/database/worlds.sql b/database/worlds.sql index 2880e15df..02b1eec96 100644 --- a/database/worlds.sql +++ b/database/worlds.sql @@ -210,6 +210,8 @@ COPY public.worlds (id, name, scene_id) FROM stdin; 85 XLeRobot Warehouse 58 86 XLeRobot Home 81 87 MMO-500 Warehouse 82 +88 Mir100 Physical 83 +89 Warehouse 1 Mir100 58 \. -- @@ -284,6 +286,8 @@ COPY public.worlds_robots (id, world_id, robot_id, poses) FROM stdin; 63 85 40 {{0.0,0.0,0.1,0.0,0.0,0.0}} 64 86 40 {{-1.6,0.3,0.1,0.0,0.0,3.14}} 65 87 41 {{0.0,0.0,0.02,0.0,0.0,0.0}} +66 88 42 {{0.0,0.0,0.0,0.0,0.0,0.0}} +67 89 36 {{0.0,0.0,0.1,0.0,0.0,0.0}} \. -- @@ -335,6 +339,7 @@ COPY public.scenes (id, name, launch_file_path, tools_config, ros_version, type, 80 Follow Turtlebot /opt/jderobot/Launchers/follow_turtlebot.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_turtlebot.config"} ROS2 gz follow_turtlebot.urdf 81 XLeRobot Home /opt/jderobot/Launchers/xlerobot_home.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/xlerobot_home.config"} ROS2 gz xlerobot_home.urdf 82 Mobile Manipulation Warehouse /opt/jderobot/Launchers/mobile_manipulation_warehouse.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/mobile_manipulation_warehouse.config"} ROS2 gz mobile_manipulation_warehouse.urdf +83 Mir100 Physical /opt/jderobot/Launchers/physical.launch.py None ROS2 physical mir100.urdf \. -- @@ -383,6 +388,7 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config, model_path 39 Mir100 High Noise /home/ws/src/CustomRobots/mir100/launch/mir100.launch.py mir100 noise:=high namespace:=mir100 mir100/models/mir100/mir100.urdf.xacro 40 XLeRobot /home/ws/src/CustomRobots/xlerobot/launch/xlerobot.launch.py xlerobot namespace:=logistic_robot xlerobot/models/xlerobot/xlerobot.urdf.xacro 41 MMO-500 /home/ws/src/CustomRobots/mmo500/launch/mmo500.launch.py mmo500 namespace:=mmo500 mmo500/models/mmo500/mmo500.urdf.xacro +42 Mir100 Physical /home/ws/src/CustomRobots/mir100/launch/mir100_physical.launch.py mir100 namespace:=mir100 mir100/models/mir100/mir100.urdf.xacro \. --