Fixed firmware and toid_bot_description

Dockerfile.base.env

@@ -1,4 +1,4 @@
-FROM arm64v8/ros:jazzy as magrob.env
+FROM arm64v8/ros:jazzy-ros-base AS magrob.base.env
 
 RUN mkdir -p /ros_ws/src
 WORKDIR /ros_ws/src
@@ -7,9 +7,7 @@ RUN --mount=type=bind,source=./,target=/ros_ws/src/ \
     --mount=type=cache,target=/var/cache/apt,sharing=locked \
     --mount=type=cache,target=/var/lib/apt,sharing=locked \
     . /opt/ros/jazzy/setup.sh && \
-    apt update && \
+    scripts/install_base_deps.sh
-    apt-get install -y ros-jazzy-hardware-interface && \
-    rosdep install -i --from-paths . -y
 
 RUN cat <<EOF >> /root/.bashrc
 source /opt/ros/jazzy/setup.bash

docker-compose.yaml

@@ -1,9 +1,9 @@
 services:
   magrob_base:
     network_mode: host
-    image: localhost/magrob.env
+    image: localhost/magrob.base.env
     build: 
-      dockerfile: Dockerfile.env
+      dockerfile: Dockerfile.base.env
 
     entrypoint: ["sleep","infinity"]
     volumes:

firmware/base/src/config.h

@@ -1,5 +1,9 @@
 #pragma once
 
+// STEP_PIN 26
+// DIR_PIN 27
+
+
 //############## CONFIG STEPPERS ################
 #define DIR_PIN_A   26
 #define DIR_PIN_B   16
@@ -10,7 +14,7 @@
 #define SM_B 1
 
 #define PULSE_PER_REV (1 / 3200.0)
-#define SPEED_SET_TIMEOUT 1000
+#define SPEED_SET_TIMEOUT 1000000
 //###############################################
 
 //================ CONFIG ENCODERS =====================
@@ -20,7 +24,7 @@
 #define ENCODER_LEFT_PIN_B  7
 #define ENCODER_CPR         3840
 
-#define WHEEL_RADIUS     0.0279
+#define WHEEL_RADIUS     0.0300
-#define WHEEL_SEPARATION 0.310735
+#define WHEEL_SEPARATION 0.264
 #define TIMER_CYCLE_US   1000
 //======================================================

firmware/base/src/main.c

@@ -213,8 +213,6 @@ void stepper_fifo(char c, uint8_t itf) {
         double vr = btod(stepper_instr + 10);
 
         set_speeds(vl, vr);
-        last_speed_change = SPEED_SET_TIMEOUT;
-
         // tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
         // tud_cdc_n_write_flush(itf);
     }
@@ -265,10 +263,6 @@ int main()
     
     
     while (true) {
-        if(last_speed_change--) {
-            set_speeds(0,  0);
-            last_speed_change = SPEED_SET_TIMEOUT;
-        }
         tud_task();
         sleep_ms(1);
     }

firmware/base/src/stepper.c

@@ -34,7 +34,7 @@ void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq) {
 
 void update_sm(PIO pio, uint sm, const uint pin ,double v) {
     double u_v = fabs(v);
-    if(u_v > 0.0005)
+    if(u_v > 0.00005)
         pio->txf[sm] = (int)((double)clock_get_hz(clk_sys) * PULSE_PER_REV / u_v) / 2 - 5;
     else
         pio->txf[sm] = 0;

mg_bringup/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>launch</depend>
   <depend>launch_ros</depend>
+  <depend>xacro</depend>
   <depend>mg_control</depend>
   <depend>mg_odometry</depend>
   <depend>mg_navigation</depend>

mg_control/assets/toid_general_params.yaml

@@ -18,5 +18,5 @@ diffdrive_controller:
 
     open_loop: true 
 
-    wheel_separation: 0.258
+    wheel_separation: 0.192
-    wheel_radius: 0.0375
+    wheel_radius: 0.032

mg_control/launch/toid.launch.py

@@ -48,7 +48,7 @@ def generate_launch_description():
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
-        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_sim:=', use_mock])}]
+        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
     )
 
     controller_manager = Node(

mg_control/src/mg_control.cpp

@@ -31,6 +31,8 @@ CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::Stat
                      "Failed to open serial port (Is the stepper driver plugged in)");
         return CallbackReturn::ERROR;
     }
+    RCLCPP_INFO(rclcpp::get_logger("MgStepperInterface"),
+                     "Configured stepper motor interface");
     return CallbackReturn::SUCCESS;
 }
 
@@ -45,8 +47,8 @@ std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_s
 
     std::vector<hardware_interface::StateInterface> state_interfaces;
     state_interfaces.reserve(2);
-    state_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
+    state_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
-    state_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
+    state_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
 
     return state_interfaces;
 }
@@ -55,8 +57,8 @@ std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export
 
     std::vector<hardware_interface::CommandInterface> command_interfaces;
     command_interfaces.reserve(2);
-    command_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
+    command_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
-    command_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
+    command_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
 
     return command_interfaces;
 }

scripts/build_base.sh

@@ -0,0 +1,10 @@
+source /opt/ros/jazzy/setup.bash
+
+colcon build --packages-select \
+    mg_bringup \
+    mg_navigation \
+    mg_msgs \
+    mg_control \
+    mg_odometry \
+    mg_obstacles \
+    toid_bot_description

scripts/docker_build.sh

@@ -0,0 +1,55 @@
+usage() {
+	cat <<EOF
+Usage:
+	docker_build.sh <target> <registry> [<args>]
+	docker_build.sh -h
+
+	Build and push a docker image to a arm64v8 registry
+
+	Targets: 
+		base     The pacakges aimed at running on the raspberry pi
+EOF
+}
+
+
+TARGET="$1"
+REGISTRY="$2"
+ARGS="$2"
+
+if [[ $TARGET == "-h" || $TARGET == "--help" ]]; then
+	usage
+	exit 0
+fi
+
+if [[ $# -lt 2 ]]; then
+	echo "Error: Expected at least 2 arguments."
+	usage
+	exit 1
+fi
+
+trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
+
+case $TARGET in
+	"base_env") 
+		ssh -N \
+			-o StrictHostKeyChecking=no \
+			-o UserKnownHostsFile=/dev/null \
+			-L 0.0.0.0:5000:localhost:5000 \
+			$REGISTRY &
+
+		echo "Building target: base"
+
+		sudo docker buildx build \
+			--platform linux/arm64 \
+			-t localhost:5000/magrob.base.env:latest \
+			-f Dockerfile.base.env \
+			--output type=registry \
+			.
+		;;
+	*)
+		echo "Target not defined"
+		exit 1
+		;;
+esac
+
+

scripts/install_base_deps.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+
+ros_distro="jazzy"
+script_dir=$(dirname "$(readlink -f "${bash_source[0]}")")
+
+source "/opt/ros/${ROS_DISTRO}/setup.bash"
+apt update
+apt-get install -y ros-jazzy-hardware-interface # For some reason this package is problamatic
+rosdep install -i -y \
+    --from-paths ./mg_bringup \
+    --from-paths ./mg_msgs \
+    --from-paths ./mg_navigation \
+    --from-paths ./mg_control \
+    --from-paths ./mg_odometry \
+    --from-paths ./mg_obstacles \
+    --from-paths ./mg_lidar

scripts/test_wheel.sh

@@ -0,0 +1,78 @@
+#!/bin/bash
+
+TARGET="$1"
+
+if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then
+    exit 0
+fi
+
+if [[ $# -ne 1 ]]; then
+    echo "Error: Expected at least 1 arg"
+    exit 1
+fi
+
+source install/setup.bash
+
+trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
+
+case $TARGET in
+    "forward") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.4
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.0"
+        ;;
+    "backward") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: -0.8
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.0"
+        ;;
+    "left") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.0
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.4"
+        ;;
+    "right") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.0
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: -0.4"
+        ;;
+    "zero")
+        ros2 service call /zero std_srvs/srv/Empty
+        ;;
+    *)
+        echo "Target not defined"
+        exit 1
+        ;;
+esac

toid_bot_description/src/toid_bot_control.xacro

@@ -18,14 +18,10 @@
 
       <joint name="drivewhl_l_joint">
         <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
       </joint>
 
       <joint name="drivewhl_r_joint">
         <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
       </joint>
 
     </ros2_control>

util/docker_registry/config/config.yml

@@ -0,0 +1,9 @@
+version: 0.1
+storage:
+  filesystem:
+    rootdirectory: /var/lib/registry
+http:
+  addr: :5000
+  headers:
+    X-Content-Type-Options: [nosniff]
+

util/docker_registry/docker-compose.yaml

@@ -0,0 +1,13 @@
+services:
+  registry:
+    restart: always
+    image: registry:2
+    ports:
+      - 5000:5000
+    environment:
+      REGISTRY_HTTP_TLS_CERTIFICATE: /certs/domain.crt
+      REGISTRY_HTTP_TLS_KEY: /certs/domain.key
+      REGISTRY_STORAGE_FILESYSTEM_ROOTDIRECTORY: /var/lib/registry
+    volumes:
+      - ./config/config.yml:/etc/docker/registry/config.yml:ro
+      - ./data:/var/lib/registry:rw