refactored for better digital shadow example and fixed minor errors

PiCar-X/Dockerfile

@@ -1,15 +1,15 @@
 ARG TAG=latest
 ARG ARCH=''
 
-FROM ${ARCH:+${ARCH}/}ros:noetic as rosnoetic
+FROM ${ARCH:+${ARCH}/}ros:noetic AS rosnoetic
 WORKDIR /root/catkin_ws/
 RUN apt update -y \
     && apt-get install -y python3-pip python3-setuptools python3-catkin-tools wget unzip curl nano \
     && apt-get clean \
     && update-ca-certificates -f \
     && pip3 install pytest pytest-cov avro
 
-FROM rosnoetic as adtf
+FROM rosnoetic AS adtf
 RUN mkdir -p ./src/arches \
     && wget -c https://git.geomar.de/open-source/arches/arches_msgs/-/archive/master/arches_msgs-master.tar.gz -O - | tar -xz \
     && mv arches_msgs-master ./src/arches/arches_msgs \

PiCar-X/core/picarx/src/picarx/drivers/clutchgear.py

@@ -16,7 +16,8 @@ def __init__(self, pwm_pin: str, i2c_port: str = '/dev/i2c-1', address=20):
         :param pwm_pin: The PWM pin configuration.
         :param i2c_port: The I2C port configuration.
         """
-        self.pwm_pin = {'channel': pwm_pin, 'i2c_port': i2c_port, 'address': address}
+        self.pwm_pin = {'channel': pwm_pin,
+                        'i2c_port': i2c_port, 'address': address}
         self.angle = 90
 
     @property
@@ -53,7 +54,7 @@ def angle(self):
     def angle(self, angle: Union[int, float]):
         """
         Set the angle.
-        
+
         The clutchgear can only handles angles between 0 and 180 degress.
 
         :param angle: The angle.
@@ -70,7 +71,7 @@ def angle(self, angle: Union[int, float]):
     def angle_to_pulse_width(self, angle):
         """
         Convert an angle to a pulse width. 
-        
+
         The pulse width is calculated using the formula: pulse_width = angle / 180 * (maximum_pulse - minimum_pulse) + minimum_pulse.
 
         :param angle: The angle.
@@ -86,7 +87,7 @@ def angle_to_pulse_width(self, angle):
     def pulse_width_to_angle(self, pulse_width):
         """
         Convert a pulse width to an angle. 
-        
+
         The angle is calculated using the formula: angle = (pulse_width - minimum_pulse) / (maximum_pulse - minimum_pulse) * 180.
 
         :param pulse_width: The pulse width.
@@ -99,6 +100,14 @@ def pulse_width_to_angle(self, pulse_width):
              SunFounderClutchGear.MINIMUM_PULSE.value)
         return round(angle)
 
+    def rotate_by_pulse_width(self, pulse_width: int) -> None:
+        """
+        Rotate the clutch gear by a given pulse width.
+
+        :param pulse_width: The pulse width.
+        """
+        self.pwm_pin.pulse_width = pulse_width
+
     @abstractmethod
     def start(self):
         """
@@ -123,4 +132,4 @@ def rotate(self, angle):
         :param angle: The angle.
         """
         raise NotImplementedError(
-            "The method {} is not implemented.".format('rotate'))
+            "The method {} is not implemented.".format('rotate'))

PiCar-X/core/picarx/src/picarx/drivers/dcmotor.py

@@ -161,11 +161,20 @@ def speed(self, speed: int):
 
     def drive_with_speed(self, speed: int):
         """
-        Drive the motor with a given speed.
+        Drive the motor with a given speed in percentage.
 
         :param speed: The speed of the motor.
         """
         self.speed = speed
+
+    def drive_with_pulse_width(self, pulse_width: int):
+        """
+        Drive the motor with a given pulse width.
+
+        :param pulse_width: The pulse width of the motor.
+        """
+        self.pwm_pin.pulse_width = pulse_width
+
 
     @abstractmethod
     def start(self):

PiCar-X/core/picarx/src/picarx/emulators/clutchgear.py

@@ -2,8 +2,7 @@
 from typing import Union
 from picarx.interfaces.actuators import ClutchGearInterface, SunFounderClutchGear
 from picarx.pwm import PWM
-from twisted.internet import reactor
-import time
+
 
 
 class AbstractClutchGearEmulator(ClutchGearInterface):
@@ -128,6 +127,7 @@ def rotate_by_angle(self, angle: Union[int, float]):
         """
         self.rotate(angle)
 
+    @abstractmethod
     def rotate(self, angle):
         """
         Rotate the clutch gear by a given angle.
@@ -137,13 +137,14 @@ def rotate(self, angle):
         raise NotImplementedError(
             "The method {} is not implemented.".format('__rotate'))
 
+    @abstractmethod
     def start(self):
         """
         Start the clutch gear emulator.
         """
         raise NotImplementedError(
             "The method {} is not implemented.".format('start'))
-
+    @abstractmethod
     def stop(self):
         """
         Stop the clutch gear emulator.

PiCar-X/docker-compose-ds.yml

@@ -51,7 +51,7 @@ services:
       picarx-ds:
         condition: service_healthy
 
-  picarx-gazebo-control:
+  picarx-ds-gazebo-control:
     image: ghcr.io/cau-se/arches-picar-x/picarx-gazebo:${TAG:-latest}
     env_file:
       - ./env/picarx-ds.env
@@ -85,12 +85,13 @@ services:
         - ARCH=${ARCH:-}
     env_file:
       - ./env/picarx-ds.env
-    command: /bin/bash -c "roslaunch arches_mqtt_bridge pt_client.launch host:=$${MQTT_HOST:-192.168.1.2}"
+    tty: true
+    command: /bin/bash -c "roslaunch arches_mqtt_bridge dt_client.launch host:=$${MQTT_HOST:-192.168.1.2}"
     depends_on:
       picarx-ds:
         condition: service_healthy
 
-  ackermann_skill-ds:
+  drive_monitor:
     image: ghcr.io/cau-se/arches-picar-x/skills/ackermann:${TAG:-latest}
     build:
       dockerfile: Dockerfile
@@ -102,23 +103,6 @@ services:
       - ./env/picarx-ds.env
     tty: true
     privileged: true
-    volumes:
-      - /sys/class/gpio:/sys/class/gpio
-      - ${I2C:-/dev/i2c-0}:${I2C:-/dev/i2c-0}
-      - ./core:/root/catkin_ws/src/core
-      - ./ros/skills/ackermann_drive:/root/catkin_ws/src/skills/ackermann_drive
-    command: /bin/bash -c "roslaunch picarx_ackermann_drive ackermann_skill.launch"
-    depends_on:
-      picarx-ds:
-        condition: service_healthy
-
-  drive_monitor:
-    build: ./ros/skills/ackermann_drive
-    image: ghcr.io/cau-se/arches-picar-x/skills/ackermann:${TAG:-latest}
-    env_file:
-      - ./env/picarx-ds.env
-    tty: true
-    privileged: true
     volumes:
       - ./core:/root/catkin_ws/src/core
       - ./ros/skills/ackermann_drive:/root/catkin_ws/src/skills/ackermann_drive

PiCar-X/docker-compose-dt.yml

@@ -70,7 +70,7 @@ services:
       - ./core:/root/catkin_ws/src/core
       - ./ros/skills/ackermann_drive:/root/catkin_ws/src/skills/ackermann_drive
     command: /bin/bash -c "
-      roslaunch picarx_ackermann_drive ackermann_skill.launch"
+      roslaunch picarx_ackermann_drive ackermann_skill_dt.launch"
     depends_on:
       picarx-dt:
         condition: service_healthy

PiCar-X/docker-compose-dtsim-same-system.yml

@@ -130,7 +130,7 @@ services:
       - ${I2C:-/dev/i2c-0}:${I2C:-/dev/i2c-0}
       - ./core:/root/catkin_ws/src/core
       - ./ros/skills/ackermann_drive:/root/catkin_ws/src/skills/ackermann_drive
-    command: /bin/bash -c "roslaunch picarx_ackermann_drive ackermann_skill.launch"
+    command: /bin/bash -c "roslaunch picarx_ackermann_drive ackermann_skill_dt.launch"
     depends_on:
       picarx-dt:
         condition: service_healthy

PiCar-X/ros/drivers/clutchgear/nodes/ackermann_clutchgear.py

@@ -53,12 +53,11 @@ class AckermannClutchGearDriver(AbstractClutchGearDriver):
     :type frequency: int, optional
     """
 
-    def __init__(self, name: str, pwm_pin: str, i2c_port: str, frequency: int = 50, address: int = 20):
+    def __init__(self, name: str, pwm_pin: str, i2c_port: str, address: int = 20, frequency: int = 50):
         super(AckermannClutchGearDriver, self).__init__(
             pwm_pin, i2c_port, address)
         self.name = name
         self.frequency = frequency
-        self.command_subscriber = None
         self.status_publisher = None
 
     def rotate(self, ros_msgs: Int8):
@@ -85,7 +84,7 @@ def turn_left(self, angle: int):
         rate = rospy.Rate(50)
         for i in range(90, aimed_angle-1, -1):
             pulse_width = self.angle_to_pulse_width(i)
-            self.pwm_pin.pulse_width = pulse_width
+            self.rotate_by_pulse_width(pulse_width)
             rate.sleep()
         self.send_status()
 
@@ -100,7 +99,7 @@ def turn_right(self, angle: int):
         rate = rospy.Rate(50)
         for i in range(90, aimed_angle+1, 1):
             pulse_width = self.angle_to_pulse_width(i)
-            self.pwm_pin.pulse_width = pulse_width
+            self.rotate_by_pulse_width(pulse_width)
             rate.sleep()
         self.send_status()
 
@@ -118,15 +117,25 @@ def stop(self):
         """
         rospy.loginfo("Shutting Ackermann steering driver down")
 
+    def rotate_from_status(self, ros_msg: ClutchGearStatus):
+        """
+        Rotates the clutch gear based on the received status message from the physical twin.
+
+        :param ros_msg: The ROS message containing the clutch gear status.
+        :type ros_msg: ClutchGearStatus
+        """
+        self.rotate_by_pulse_width(ros_msg.pulsewidth)
+
     def start(self):
         """
         Starts the driver and initializes ROS node and publishers.
         """
         rospy.init_node(self.name, anonymous=True)
         rospy.loginfo("Ackermann steering driver initialized")
         rospy.on_shutdown(self.stop)
-        self.command_subscriber = rospy.Subscriber(
-            rospy.get_param('~steering_topic'), Int8, callback=self.rotate)
+        rospy.Subscriber(rospy.get_param('~steering_topic'), Int8, callback=self.rotate)
+        rospy.Subscriber(rospy.get_param('~steering_topic')+'/from_status', ClutchGearStatus, self.rotate_from_status)
+
         self.status_publisher = rospy.Publisher(
             rospy.get_param('~steering_status_topic'), ClutchGearStatus, queue_size=5)
         rospy.spin()

PiCar-X/ros/drivers/dcmotor/nodes/dcmotor_driver.py

@@ -81,7 +81,7 @@ def __init__(self, name: str, direction_pin: Union[int, str], pwm_pin: Union[int
                                             pwm_pin, i2c_port, MotorSide(int(motor_side)), address)
         self.status_publisher = None
 
-    def drive(self, ros_msg):
+    def drive(self, ros_msg: Int8):
         """
         Drives the motor based on the received ROS message.
 
@@ -94,12 +94,22 @@ def drive(self, ros_msg):
         time.sleep(0.05)
         self.send_status()
 
+    def drive_pulse_width(self, ros_msg: MotorStatus):
+        """
+        Drives the motor based on the received ROS message.
+
+        :param ros_msg: ROS message containing the speed data.
+        :type ros_msg: Int8
+        """
+        self.direction = TravelDirection.FORWARD if ros_msg.direction == 0 else TravelDirection.BACKWARD
+        self.drive_with_pulse_width(ros_msg.pulse_width)
+
     def send_status(self):
         """
         Sends the current status of the motor.
         """
         status_message = MotorStatus(
-            location=self.motor_side.value, direction=self.direction.value, speed=self.speed)
+            location=self.motor_side.value, direction=self.direction.value, pulse_width=self.speed)
         self.status_publisher.publish(status_message)
 
     def start(self):
@@ -109,6 +119,7 @@ def start(self):
         rospy.init_node(self.name, anonymous=False)
         rospy.on_shutdown(self.stop)
         rospy.Subscriber(rospy.get_param('~command_topic'), Int8, self.drive)
+        rospy.Subscriber(rospy.get_param('~command_topic')+'/from_status', MotorStatus, self.drive_pulse_width)
         self.status_publisher = rospy.Publisher(
             rospy.get_param('~status_topic'), MotorStatus, queue_size=5)
         rospy.spin()

PiCar-X/ros/emulators/clutchgear/nodes/ackermann_clutchgear_emulator.py

@@ -52,9 +52,9 @@ class AckermannClutchGearEmulator(AbstractClutchGearEmulator):
     :type frequency: int, optional
     """
 
-    def __init__(self, name: str, pwm_pin: str, i2c_port: str, frequency: int = 50, address: int = 20):
+    def __init__(self, name: str, pwm_pin: str, i2c_port: str, address: int = 20, frequency: int = 50):
         super(AckermannClutchGearEmulator, self).__init__(
-            pwm_pin, i2c_port, address)
+            pwm_pin, i2c_port, address=address)
         self.name = name
         self.frequency = frequency
         self.left_steer = None
@@ -169,7 +169,7 @@ def start(self):
             '~right_steer_topic'), Float64, queue_size=5)
         self.wheel_base = float(rospy.get_param("~wheel_base"))
         self.wheel_track = float(rospy.get_param("~wheel_track"))
-        frequency = rospy.Rate(50)  # 50Hz
+        frequency = rospy.Rate(50)  # default 50Hz
         while not rospy.is_shutdown():
             self.rotate(self.pwm_pin.pulse_width)
             frequency.sleep()

PiCar-X/ros/picarx_msgs/msg/DriveStatus.msg

@@ -1,4 +1,3 @@
-arches_msgs/HeaderTwin header
 time timestamp
 MotorStatus motor_left
 MotorStatus motor_right

PiCar-X/ros/picarx_msgs/msg/MotorStatus.msg

@@ -8,4 +8,4 @@ uint8 MOTOR_RIGHT = 1
 
 uint8 location # motor left or motor right
 int8 direction # FORWARD or BACKWARD
-int16 speed
+int16 pulse_width

PiCar-X/ros/skills/ackermann_drive/CMakeLists.txt

@@ -33,6 +33,7 @@ install(DIRECTORY
 install(PROGRAMS
   nodes/ackermann_drive_skill.py
   nodes/ackermann_monitor_ds.py
+  nodes/ackermann_monitor_skill.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 if(CATKIN_ENABLE_TESTING)

PiCar-X/ros/skills/ackermann_drive/launch/ackermann_monitoring_skill.launch

@@ -0,0 +1,17 @@
+<launch>
+    <arg name="motor_left_topic" default="motor_left/speed" />
+    <arg name="motor_right_topic" default="motor_right/speed" />
+    <arg name="steering_topic" default="steering/angle" />
+    <arg name="status_topic" default="drive/status" />
+
+    <arg name="name" default="AckermannMonitoringSkill" />
+    <arg name="uid" default="ackermannmonitoringskill" />
+
+    <node name="ackermann_monitoring_skill" pkg="picarx_ackermann_drive" type="ackermann_monitor_skill.py" respawn="false" output="screen" args="$(arg name) $(arg uid)">
+      <param name="motor_left_topic" type="str" value="$(arg motor_left_topic)" />
+      <param name="motor_right_topic" type="str" value="$(arg motor_right_topic)" />
+      <param name="steering_topic" type="str" value="$(arg steering_topic)" />
+      <param name="status_topic" type="str" value="$(arg status_topic)" />
+    </node>
+
+  </launch>

PiCar-X/ros/skills/ackermann_drive/launch/ackermann_skill.launch

@@ -6,7 +6,7 @@
   <arg name="motor_left_status_topic" default="/motor_left/status" />
   <arg name="motor_right_status_topic" default="/motor_right/status" />
   <arg name="steering_status_topic" default="/steering/status" />
-  <arg name="status_topic" default="drive/status" />
+  <arg name="status_topic" default="/drive/status" />
 
   <arg name="name" default="AckermannSkill" />
   <arg name="uid" default="ackermannskill" />