Sync Svea Ros2 to EL2425 Progress

.github/workflows/update-ghcr.yml

@@ -12,6 +12,7 @@ permissions:
 jobs:
 
   build-and-push:
+    if: ${{ github.repository == 'KTH-SML/svea' }}
     runs-on: ubuntu-latest
     steps:
     - name: Checkout code

docker/Dockerfile

@@ -20,7 +20,7 @@ RUN apt-get update -y
 
 # This is for any added ROS packages
 COPY src ./src
-COPY requirements.txt ./
+COPY entrypoint requirements.txt ./
 RUN rosdep install \
         --rosdistro $ROSDISTRO \
         --from-paths src \

docker/Dockerfile.base

@@ -69,7 +69,7 @@ RUN mkdir -p /var/run/sshd
 # Bash reminder:
 #   username="${creds%%:*}"  # remove everything after the first ':'
 #   password="${creds#*:}"   # remove everything before the first ':'
-RUN echo "root:${USER_CREDENTIALS#*:}" | chpasswd
+RUN echo "${USER_CREDENTIALS%%:*}:${USER_CREDENTIALS#*:}" | chpasswd
 
 # SSH config: allow root + password login (keep pubkey auth on too)
 RUN sed -ri 's/^#?PasswordAuthentication .*/PasswordAuthentication yes/' /etc/ssh/sshd_config && \
@@ -99,6 +99,12 @@ RUN if [ -f src/mocap4ros2_qualisys/dependency_repos.repos ]; then \
       vcs import src < src/mocap4ros2_qualisys/dependency_repos.repos; \
     fi
 
+# Zenoh
+RUN echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo/ /" | tee -a /etc/apt/sources.list > /dev/null && \
+    apt update -y && \
+    apt install --no-install-recommends -y zenoh-bridge-ros2dds && \
+    rm -rf /var/lib/apt/lists/*
+
 # Install dependencies via rosdep and pip
 RUN apt-get update -y && \
     rosdep update \
@@ -141,4 +147,4 @@ RUN chmod +x ./entrypoint
 
 ENTRYPOINT ["./entrypoint"]
 
-CMD ["bash"]
+CMD ["bash"]

entrypoint

@@ -24,6 +24,6 @@ if [ "$MICROROS_DISABLE_SHM" = "1" ] ; then
 fi
 
 # To build anything that has been added by volume...
-colcon build --symlink-install
+colcon build --symlink-install || true
 
 exec "$@"

requirements.txt

@@ -8,7 +8,8 @@
 # You can read more here: https://pip.pypa.io/en/stable/reference/requirements-file-format/#requirements-file-format
 
 numpy
-matplotlib
+matplotlib # deprecated
+pillow
 
 # This is for MPC
 casadi==3.6.7

src/el2425/launch/traffic_light.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="name"            default="light_a"/>
+    <arg name="alpha"           default="1.0"/>
+    <arg name="pos_x"           default="0.0"/>
+    <arg name="pos_y"           default="0.0"/>
+
+    <node name="$(var name)" pkg="el2425" exec="traffic_light.py" output="screen">
+        <param name="alpha" value="$(var alpha)"/>
+        <param name="pos_x" value="$(var pos_x)"/>
+        <param name="pos_y" value="$(var pos_y)"/>
+    </node>
+</launch>

src/el2425/package.xml

@@ -0,0 +1,16 @@
+<?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>el2425</name>
+  <version>0.0.0</version>
+  <description>The el2425 package</description>
+  <maintainer email="kajarf@kth.se">Kaj Munhoz Arfvidsson</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>svea_core</depend>
+  <depend>rclpy</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/el2425/scripts/traffic_light.py

@@ -0,0 +1,107 @@
+#! /usr/bin/env python3
+
+# imports 
+import random
+
+from rclpy.clock import Clock, Duration
+from std_msgs.msg import String, Float32
+
+from svea_core import rosonic as rx
+from svea_core.interfaces import ShowMarker
+
+class traffic_light(rx.Node):
+
+    ## Constants
+
+    GREEN_TIME  = 5.                # [s]
+    YELLOW_TIME = 2.                # [s]
+    RED_TIME    = 5.                # [s]
+
+    # dictionary for states
+    TRANSITIONS = {
+        'Rd': 'YG', # Red           -> Yellow-Green
+        'YG': 'Gr', # Yellow-Green  -> Green
+        'Gr': 'YR', # Green         -> Yellow-Red
+        'YR': 'Rd', # Yellow-Red    -> Red
+    }
+
+    ## Parameters
+
+    rate = rx.Parameter(10)
+    alpha = rx.Parameter(1.0)
+
+    pos_x = rx.Parameter(0.0)
+    pos_y = rx.Parameter(0.0)
+
+    ## Publishers
+
+    state_pub = rx.Publisher(String, '~/state')
+    time_pub = rx.Publisher(Float32, '~/time_left')
+
+    ## Interfaces
+
+    marker = ShowMarker()
+
+    ## Methods
+
+    def on_startup(self):
+
+        assert 0 <= self.alpha <= min(self.RED_TIME, self.GREEN_TIME)
+
+        self.switch('Rd')  # initial state is red
+        self._loop_tmr = self.create_timer(1.0 / self.rate, self.loop)
+
+    def loop(self):
+
+        now = Clock().now()
+
+        # calculate time remaining for current state
+        time_left = (self._time0 + self._delta - now).nanoseconds / 1e9
+
+        # if time has elapsed, then switch to next state
+        if time_left <= 0:
+            self.switch()
+
+        # publish current state and time remaining
+        msg = String()
+        msg.data = self._state
+        self.state_pub.publish(msg)
+
+        msg = Float32()
+        msg.data = time_left
+        self.time_pub.publish(msg)
+
+        # visualization for the lights
+        self.visualize_traffic_lights()
+
+    def switch(self, to=None):
+        """
+        Change the traffic light to the next state
+        """
+        self._state = (to if to is not None else
+                       self.TRANSITIONS[self._state])
+
+        delta = (self.RED_TIME      if self._state == 'Rd' else
+                 self.GREEN_TIME    if self._state  == 'Gr' else
+                 self.YELLOW_TIME)
+        if self._state in ('Rd', 'Gr'):
+            delta += random.uniform(-self.alpha, +self.alpha)
+
+        # delta determines how long each state lasts
+        self._delta = Duration(seconds=delta)
+
+        self._time0 = Clock().now()
+
+    def visualize_traffic_lights(self):
+        """Publish the positions of traffic lights for visualization in RViz."""
+
+        color = ('red' if self._state == 'Rd' else
+                 'green' if self._state == 'Gr' else
+                 'yellow' if self._state in ('YG', 'YR') else
+                 'black')
+
+        self.marker.place([self.pos_x, self.pos_y, 1.0], color=color)
+
+if __name__ == '__main__':
+
+    traffic_light.main()

src/el2425/setup.py

@@ -0,0 +1,27 @@
+import os
+from glob import glob
+import xml.etree.ElementTree as ET
+from setuptools import find_packages, setup
+
+package = ET.parse('package.xml').getroot()
+name = package.find('name').text
+
+setup(
+    name=name,
+    version='0.0.0',
+    packages=find_packages(include=[name, f"{name}.*"]),
+    data_files=[
+        ('share/ament_index/resource_index/packages', [f'resource/{name}']),
+        (f'share/{name}', ['package.xml']),
+        (f'share/{name}/launch', glob('launch/*.xml')),
+        (f'lib/{name}', glob('scripts/*.py')),
+        (os.path.join('share', name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer=package.find('maintainer').text,
+    maintainer_email=package.find('maintainer').get('email'),
+    description=package.find('description').text,
+    license=package.find('license').text,
+    tests_require=['pytest'],
+)

src/svea_core/launch/map_and_foxglove.xml

@@ -1,19 +1,20 @@
 <?xml version="1.0"?>
 <launch>
-    <!-- Launch file arguments -->
+    <!-- Arguments BEGIN -->
+
     <arg name="map"             default="floor2"/>
-    <arg name="map_path" default="$(find-pkg-share svea_core)/maps/$(var map).yaml"/>
+    <arg name="map_file"        default="$(find-pkg-share svea_core)/maps/$(var map).yaml"/>
     <arg name="use_foxglove"    default="true"/>
-    <let name="use_foxglove" value="$(eval '\'$(var use_foxglove)\' == \'true\'')"/>
+
+    <!-- Arguments END -->
 
     <!-- Start map server -->
     <!-- more info about nav2_map_server:https://answers.ros.org/question/398095/-->
     <node name="map_server" pkg="nav2_map_server" exec="map_server" output="screen">
-        <param name="yaml_filename" value="$(var map_path)" />
+        <param name="yaml_filename" value="$(var map_file)" />
         <param name="use_sim_time" value="false" />
         <param name="topic_name" value="map" />
     </node>
-
     <node name="lifecycle_manager" pkg="nav2_lifecycle_manager" exec="lifecycle_manager" output="screen">
         <param name="node_names" value="['map_server']" />
         <param name="autostart" value="true" />

src/svea_core/launch/simulation.xml

@@ -1,16 +1,38 @@
 <launch>
+    <!-- Arguments BEGIN -->
+
+    <arg name="name"            default="self"/>
     <arg name="map"             default="floor2"/>
     <arg name="obstacle_map"    default="$(var map)_obstacles"/>
-    <arg name="state" default="[-7.4, -15.3, 0.9, 0.0]" />
-    <arg name="is_sim" default="true"/>
+    <arg name="initial_pose_x"  default="0.0" />
+    <arg name="initial_pose_y"  default="0.0" />
+    <arg name="initial_pose_a"  default="0.0" />
+
+    <!-- Frames -->
+    <arg name="map_frame"   default="map"/>
+    <arg name="odom_frame"  default="$(var name)/odom"/>
+    <arg name="base_frame"  default="$(var name)/base_link"/>
+
+    <!-- Arguments END -->
+
+    <group>
+
+        <push-ros-namespace namespace="$(var name)"/>
+
+        <node name="sim_svea" pkg="svea_core" exec="sim_svea.py" output="screen">
+            <param name="initial_pose_x" value="$(var initial_pose_x)"/>
+            <param name="initial_pose_y" value="$(var initial_pose_y)"/>
+            <param name="initial_pose_a" value="$(var initial_pose_a)"/>
+            <param name="map_frame" value="$(var map_frame)"/>
+            <param name="odom_frame" value="$(var odom_frame)"/>
+            <param name="base_frame" value="$(var base_frame)"/>
+        </node>
 
+        <node name="sim_lidar" pkg="svea_core" exec="sim_lidar.py" output="screen">
+            <param from="$(find-pkg-share svea_core)/params/$(var obstacle_map).yaml"/>
+            <param name="laser_frame" value="$(var name)/laser" />
+        </node>
 
-    <node name="sim_svea" pkg="svea_core" exec="sim_svea.py" output="screen">
-        <param name="state" value="$(var state)" />
-    </node>
+    </group>
 
-    <node name="sim_lidar" pkg="svea_core" exec="sim_lidar.py" output="screen">
-        <param from="$(find-pkg-share svea_core)/params/$(var obstacle_map).yaml"/>
-    </node>
-
 </launch>

src/svea_core/launch/svea.xml

@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<launch>
+
+    <!-- Arguments BEGIN -->
+
+    <arg name="name"            default="self"/>
+    <arg name="is_sim"          default="true"/>
+    <arg name="is_indoor"       default="true"/>
+    <arg name="initial_pose_x"  default="0.0"/>
+    <arg name="initial_pose_y"  default="0.0"/>
+    <arg name="initial_pose_a"  default="0.0"/> <!-- Yaw Angle -->
+
+    <arg name="use_zenoh"       default="false"/>
+
+    <arg name="use_urdf"        default="$(var is_sim)"/>
+
+    <arg name="map"             default="floor2"/>
+    <arg name="map_file"        default="$(find-pkg-share svea_core)/maps/$(var map).yaml"/>
+
+    <!-- Arguments END -->
+
+    <!-- Localization -->
+    <include file="$(find-pkg-share svea_localization)/launch/localization.xml">
+        <arg name="name"           value="$(var name)"/>
+        <arg name="is_sim"         value="$(var is_sim)"/>
+        <arg name="is_indoor"      value="$(var is_indoor)"/>
+        <arg name="initial_pose_x" value="$(var initial_pose_x)" />
+        <arg name="initial_pose_y" value="$(var initial_pose_y)" />
+        <arg name="initial_pose_a" value="$(var initial_pose_a)" />
+    </include>
+
+    <!-- If-Else Group -->
+    <group if="$(var is_sim)">
+        <!-- Simulation -->
+        <include file="$(find-pkg-share svea_core)/launch/simulation.xml">
+            <arg name="name"    value="$(var name)"/>
+            <arg name="map"     value="$(var map)" />
+            <arg name="initial_pose_x" value="$(var initial_pose_x)" />
+            <arg name="initial_pose_y" value="$(var initial_pose_y)" />
+            <arg name="initial_pose_a" value="$(var initial_pose_a)" />
+        </include>
+    </group>
+    <group unless="$(var is_sim)">
+        <!-- Low-Level Interface -->
+        <executable cmd="$(find-pkg-share svea_core)/util/start_micro_ros.sh" output="screen"/>
+
+        <!--LLI Fixers-->
+        <node pkg="svea_core" exec="encoder_filter.py" name="encoder_filter" output="screen">
+            <param name="frame_id"    value="$(var name)/wheel_encoder"/>
+        </node>
+
+        <node pkg="svea_core" exec="imu_bias_remover.py" name="imu_bias_remover" output="screen">
+            <param name="frame_id"    value="$(var name)/imu"/>
+        </node>
+
+        <node pkg="svea_core" exec="lidar_timer.py" name="lidar_timer" output="screen">
+            <param name="target_frame" value="$(var name)/base_link"/>
+            <param name="source_frame" value="$(var name)/odom"/>
+            <param name="from_topic"   value="$(var name)/scan"/>
+            <param name="to_topic"     value="$(var name)/scan/filtered"/>
+        </node>
+    </group>
+
+    <!-- Load Visualization Model -->
+    <include if="$(var use_urdf)" file="$(find-pkg-share svea_core)/launch/visualization.xml">
+        <arg name="name" value="$(var name)"/>
+    </include>
+
+    <!-- Zenoh -->
+    <let name="zenoh_cmd" value="zenoh-bridge-ros2dds -c $(find-pkg-share svea_core)/params/zenoh.json5 --ros-args"/>
+    <let name="zenoh_cmd" value="$(var zenoh_cmd) --remap /fleet/$(var name)/state:=/$(var name)/odometry/local"/>
+    <!-- <executable if="$(var use_zenoh)" cmd="$(var zenoh_cmd)" output="screen"/> -->
+    <let name="zenoh_config" value="$(find-pkg-share svea_core)/params/zenoh.json5"/>
+    <executable if="$(var use_zenoh)" cmd="zenoh-bridge-ros2dds -c $(var zenoh_config)" output="screen"/>
+
+</launch>