Fixed README

.dockerignore

@@ -0,0 +1,4 @@
+.git
+*.log
+build/
+install/

.env

@@ -0,0 +1 @@
+DOCKER_IMAGE=main-jazzy-tutorial-source

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "src/astra_description"]
 	path = src/astra_descriptions
 	url = ../astra_descriptions
+[submodule "src/astra_msgs"]
+	path = src/astra_msgs
+	url = git@github.com:SHC-ASTRA/astra_msgs.git

README.md

@@ -11,6 +11,7 @@ You will use these packages to launch all rover-side ROS2 nodes.
 - [Software Prerequisites](#software-prerequisites)
   - [Nix](#nix)
   - [ROS2 Humble + rosdep](#ros2-humble--rosdep)
+  - [Docker](#docker)
 - [Running](#running)
   - [Testing Serial](#testing-serial)
   - [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
@@ -47,6 +48,17 @@ $ cd path/to/rover-ros2
 $ rosdep install --from-paths src -y --ignore-src
 ```
 
+### Docker
+
+Using the docker compose file automatically builds the workspace and allows you to choose between running on the CPU or GPU for applications like Rviz2 and Gazebo:
+
+```bash
+  # Run on CPU
+$ docker compose run --rm --name rover-ros2-container cpu
+  # Run on GPU (NVidia only)
+$ docker compose run --rm --name rover-ros2-container gpu
+```
+
 ## Running
 
 ```bash

auto_start/auto_start_anchor.sh

@@ -14,8 +14,12 @@ echo "[INFO] Network interface is up!"
 # Your actual ROS node start command goes here
 echo "[INFO] Starting ROS node..."
 
-# Source ROS 2 Humble setup script (if we aren't using nix)
-command -v ros2 || source /opt/ros/humble/setup.bash
+# Source ROS 2 Humble setup script
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/auto_start_headless_full.sh

@@ -15,7 +15,11 @@ echo "[INFO] Network interface is up!"
 echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
-command -v ros2 || source /opt/ros/humble/setup.bash
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/start_rosbag.sh

@@ -17,7 +17,11 @@ done
 echo "[INFO] Network interface is up!"
 
 
-command -v ros2 || source /opt/ros/humble/setup.bash
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 source $ANCHOR_WS/install/setup.bash
 [[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash
 

docker-compose.yml

@@ -0,0 +1,38 @@
+services:
+  cpu:
+    image: moveit/moveit2:${DOCKER_IMAGE}
+    container_name: moveit2_container
+    privileged: true
+    network_mode: host
+    command: ["bash", "-c", "colcon build --symlink-install && source /ros2_ws/install/setup.bash && exec bash"]
+    volumes:
+      - ./:/ros2_ws
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      - $XAUTHORITY:/root/.Xauthority
+    working_dir: /ros2_ws
+    environment:
+      QT_X11_NO_MITSHM: 1
+      DISPLAY: $DISPLAY
+  gpu:
+    image: moveit/moveit2:${DOCKER_IMAGE}
+    container_name: moveit2_container
+    privileged: true
+    network_mode: host
+    command: ["bash", "-c", "colcon build --symlink-install && source /ros2_ws/install/setup.bash && exec bash"]
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: 1
+              capabilities: [gpu]
+    volumes:
+      - ./:/ros2_ws
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      - $XAUTHORITY:/root/.Xauthority
+    working_dir: /ros2_ws
+    environment:
+      QT_X11_NO_MITSHM: 1
+      DISPLAY: $DISPLAY
+      NVIDIA_VISIBLE_DEVICES: all
+      NVIDIA_DRIVER_CAPABILITIES: all

flake.lock

@@ -1,29 +1,5 @@
 {
   "nodes": {
-    "astra-msgs": {
-      "inputs": {
-        "nix-ros-overlay": "nix-ros-overlay",
-        "nixpkgs": [
-          "astra-msgs",
-          "nix-ros-overlay",
-          "nixpkgs"
-        ]
-      },
-      "locked": {
-        "lastModified": 1770693861,
-        "narHash": "sha256-8NBtSsKVYtd+NYt51tJ1EteC0nOTemDUBoRXbDoQAuA=",
-        "owner": "SHC-ASTRA",
-        "repo": "astra_msgs",
-        "rev": "60bbb53085b09fbdb7e848b1dd168d526d9af281",
-        "type": "github"
-      },
-      "original": {
-        "owner": "SHC-ASTRA",
-        "ref": "60bbb53085b09fbdb7e848b1dd168d526d9af281",
-        "repo": "astra_msgs",
-        "type": "github"
-      }
-    },
     "flake-utils": {
       "inputs": {
         "systems": "systems"
@@ -42,49 +18,11 @@
         "type": "github"
       }
     },
-    "flake-utils_2": {
-      "inputs": {
-        "systems": "systems_2"
-      },
-      "locked": {
-        "lastModified": 1731533236,
-        "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
-        "type": "github"
-      },
-      "original": {
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "type": "github"
-      }
-    },
     "nix-ros-overlay": {
       "inputs": {
         "flake-utils": "flake-utils",
         "nixpkgs": "nixpkgs"
       },
-      "locked": {
-        "lastModified": 1770408708,
-        "narHash": "sha256-E7ZQRgGrsiuswgXnG7337ZR5s4SdZlheZjxKOQdVoH8=",
-        "owner": "lopsided98",
-        "repo": "nix-ros-overlay",
-        "rev": "e78ba91032c7f8bdd823fbf43937cbf0f4f09747",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "develop",
-        "repo": "nix-ros-overlay",
-        "type": "github"
-      }
-    },
-    "nix-ros-overlay_2": {
-      "inputs": {
-        "flake-utils": "flake-utils_2",
-        "nixpkgs": "nixpkgs_2"
-      },
       "locked": {
         "lastModified": 1770108954,
         "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
@@ -116,26 +54,9 @@
         "type": "github"
       }
     },
-    "nixpkgs_2": {
-      "locked": {
-        "lastModified": 1759381078,
-        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
-        "owner": "NixOS",
-        "repo": "nixpkgs",
-        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "nix-ros",
-        "repo": "nixpkgs",
-        "type": "github"
-      }
-    },
     "root": {
       "inputs": {
-        "astra-msgs": "astra-msgs",
-        "nix-ros-overlay": "nix-ros-overlay_2",
+        "nix-ros-overlay": "nix-ros-overlay",
         "nixpkgs": [
           "nix-ros-overlay",
           "nixpkgs"
@@ -156,21 +77,6 @@
         "repo": "default",
         "type": "github"
       }
-    },
-    "systems_2": {
-      "locked": {
-        "lastModified": 1681028828,
-        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
-        "owner": "nix-systems",
-        "repo": "default",
-        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
-        "type": "github"
-      },
-      "original": {
-        "owner": "nix-systems",
-        "repo": "default",
-        "type": "github"
-      }
     }
   },
   "root": "root",

flake.nix

@@ -4,8 +4,6 @@
   inputs = {
     nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
-    # specify astra_msgs commit hash to the one we support
-    astra-msgs.url = "github:SHC-ASTRA/astra_msgs?ref=60bbb53085b09fbdb7e848b1dd168d526d9af281";
   };
 
   outputs =
@@ -13,7 +11,6 @@
       self,
       nix-ros-overlay,
       nixpkgs,
-      astra-msgs,
     }:
     nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
       system:
@@ -27,7 +24,6 @@
         devShells.default = pkgs.mkShell {
           name = "ASTRA Anchor";
           packages = with pkgs; [
-            astra-msgs.packages.${system}.astra-msgs
             colcon
             (python313.withPackages (
               p: with p; [
@@ -43,7 +39,6 @@
               buildEnv {
                 paths = [
                   ros-core
-                  rqt-graph
                   ros2cli
                   ros2run
                   ros2bag

src/bio_pkg/bio_pkg/bio_node.py

@@ -1,203 +1,241 @@
-import signal
+import rclpy
+from rclpy.node import Node
+import serial
 import sys
 import threading
+import os
+import glob
 import time
-
-import rclpy
-from astra_msgs.action import BioVacuum
-from astra_msgs.msg import BioControl, BioDistributor, VicCAN
-from astra_msgs.srv import BioTestTube
-from rclpy.action import ActionServer
-from rclpy.node import Node
-from std_msgs.msg import Header, String
+import atexit
+import signal
+from std_msgs.msg import String
+from astra_msgs.msg import BioControl
+from astra_msgs.msg import BioFeedback
 
 serial_pub = None
 thread = None
 
-# used to verify the length of an incoming VicCAN feedback message
-# key is VicCAN command_id, value is expected length of data list
-viccan_citadel_msg_len_dict = {
-    # empty because not expecting any VicCAN from citadel atm
-}
-viccan_lance_msg_len_dict = {
-    # empty because not sure what VicCAN commands LANCE sends
-}
-
 
 class SerialRelay(Node):
     def __init__(self):
         # Initialize node
         super().__init__("bio_node")
 
-        # Anchor Topics
-        self.anchor_fromvic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/from_vic/bio", self.relay_fromvic, 20
-        )
-        self.anchor_tovic_pub_ = self.create_publisher(
-            VicCAN, "/anchor/to_vic/relay", 20
+        # Get launch mode parameter
+        self.declare_parameter("launch_mode", "bio")
+        self.launch_mode = self.get_parameter("launch_mode").value
+        self.get_logger().info(f"bio launch_mode is: {self.launch_mode}")
+
+        # Create publishers
+        self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
+        self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)
+
+        # Create subscribers
+        self.control_sub = self.create_subscription(
+            BioControl, "/bio/control", self.send_control, 10
         )
 
-        self.anchor_sub = self.create_subscription(
-            String, "/anchor/bio/feedback", self.anchor_feedback, 10
-        )
-        self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+        # Create a publisher for telemetry
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
 
-        self.distributor_sub = self.create_subscription(
-            BioDistributor,
-            "/bio/control/distributor",
-            self.distributor_callback,
-            10,
-        )
+        # Topics used in anchor mode
+        if self.launch_mode == "anchor":
+            self.anchor_sub = self.create_subscription(
+                String, "/anchor/bio/feedback", self.anchor_feedback, 10
+            )
+            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
-        # Services
-        self.test_tube_service = self.create_service(
-            BioTestTube, "/bio/control/test_tube", self.test_tube_callback
-        )
+        self.bio_feedback = BioFeedback()
 
-        # Actions
-        self._action_server = ActionServer(
-            self, BioVacuum, "/bio/actions/vacuum", self.execute_vacuum
-        )
+        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
+        if self.launch_mode == "bio":
+            # Loop through all serial devices on the computer to check for the MCU
+            self.port = None
+            for i in range(2):
+                try:
+                    # connect and send a ping command
+                    set_port = (
+                        "/dev/ttyACM0"  # MCU is controlled through GPIO pins on the PI
+                    )
+                    ser = serial.Serial(set_port, 115200, timeout=1)
+                    # print(f"Checking port {port}...")
+                    ser.write(b"ping\n")
+                    response = ser.read_until("\n")
+
+                    # if pong is in response, then we are talking with the MCU
+                    if b"pong" in response:
+                        self.port = set_port
+                        self.get_logger().info(f"Found MCU at {set_port}!")
+                        break
+                except:
+                    pass
+
+            if self.port is None:
+                self.get_logger().info(
+                    "Unable to find MCU... please make sure it is connected."
+                )
+                time.sleep(1)
+                sys.exit(1)
+
+            self.ser = serial.Serial(self.port, 115200)
+            atexit.register(self.cleanup)
 
     def run(self):
         global thread
         thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
         thread.start()
 
+        # if in arm mode, will need to read from the MCU
+
         try:
             while rclpy.ok():
-                time.sleep(0.1)
+                if self.launch_mode == "bio":
+                    if self.ser.in_waiting:
+                        self.read_mcu()
+                    else:
+                        time.sleep(0.1)
         except KeyboardInterrupt:
             pass
+        finally:
+            self.cleanup()
+
+    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    def read_mcu(self):
+        try:
+            output = str(self.ser.readline(), "utf8")
+            if output:
+                self.get_logger().info(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+        except serial.SerialException:
+            self.get_logger().info("SerialException caught... closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except TypeError as e:
+            self.get_logger().info(f"TypeError: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except Exception as e:
+            print(f"Exception: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+
+    def send_ik(self, msg):
+        pass
 
     def send_control(self, msg: BioControl):
-        print("todo")
+        # CITADEL Control Commands
+        ################
+
+        # Chem Pumps, only send if not zero
+        if msg.pump_id != 0:
+            command = (
+                "can_relay_tovic,citadel,27,"
+                + str(msg.pump_id)
+                + ","
+                + str(msg.pump_amount)
+                + "\n"
+            )
+            self.send_cmd(command)
+        # Fans, only send if not zero
+        if msg.fan_id != 0:
+            command = (
+                "can_relay_tovic,citadel,40,"
+                + str(msg.fan_id)
+                + ","
+                + str(msg.fan_duration)
+                + "\n"
+            )
+            self.send_cmd(command)
+        # Servos, only send if not zero
+        if msg.servo_id != 0:
+            command = (
+                "can_relay_tovic,citadel,25,"
+                + str(msg.servo_id)
+                + ","
+                + str(int(msg.servo_state))
+                + "\n"
+            )
+            self.send_cmd(command)
+
+        # LSS (SCYTHE)
+        command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
+        # self.send_cmd(command)
+
+        # Vibration Motor
+        command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
+        # self.send_cmd(command)
+
+        # FAERIE Control Commands
+        ################
+
+        # To be reviewed before use#
+
+        # Laser
+        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        # self.send_cmd(command)
+
+        # Drill (SCABBARD)
+        command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
+        # self.send_cmd(command)
+
+        # Bio linear actuator
+        command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
+        self.send_cmd(command)
 
     def send_cmd(self, msg: str):
-        # send to anchor node to relay
-        output = String()
-        output.data = msg
-        self.anchor_pub.publish(output)
-
-    def relay_fromvic(self, msg: VicCAN):
-        # self.get_logger().info(msg)
-        if msg.mcu_name == "citadel":
-            self.process_fromvic_citadel(msg)
-
-    def process_fromvic_citadel(self, msg: VicCAN):
-        # Check message len to prevent crashing on bad data
-        if msg.command_id in viccan_citadel_msg_len_dict:
-            expected_len = viccan_citadel_msg_len_dict[msg.command_id]
-            if len(msg.data) != expected_len:
-                self.get_logger().warning(
-                    f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})"
-                )
-            return
+        if (
+            self.launch_mode == "anchor"
+        ):  # if in anchor mode, send to anchor node to relay
+            output = String()
+            output.data = msg
+            self.anchor_pub.publish(output)
+        elif self.launch_mode == "bio":  # if in standalone mode, send to MCU directly
+            self.get_logger().info(f"[Bio to MCU] {msg}")
+            self.ser.write(bytes(msg, "utf8"))
 
     def anchor_feedback(self, msg: String):
         output = msg.data
         parts = str(output.strip()).split(",")
-        self.get_logger().info(f"[Bio Anchor] {msg.data}")
-        # no data planned to be received from citadel, not sure about lance
+        # self.get_logger().info(f"[Bio Anchor] {msg.data}")
 
-    def distributor_callback(self, msg: BioDistributor):
-        distributor_arr = msg.distributor_id
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="citadel",
-            command_id=40,
-            data=[
-                clamp_short(distributor_arr[0]),
-                clamp_short(distributor_arr[1]),
-                clamp_short(distributor_arr[2]),
-                0,
-            ],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
+        if output.startswith(
+            "can_relay_fromvic,citadel,54"
+        ):  # bat, 12, 5, Voltage readings * 100
+            self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
+            self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
+            self.bio_feedback.voltage_5 = float(parts[5]) / 100.0
+        elif output.startswith("can_relay_fromvic,digit,57"):
+            self.bio_feedback.drill_temp = float(parts[3])
+            self.bio_feedback.drill_humidity = float(parts[4])
 
-    def test_tube_callback(self, request, response):
-        tubes_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="citadel",
-            command_id=40,
-            data=[
-                float(int.from_bytes(request.tube_id)),
-                float(request.milliliters),
-            ],
-        )
-        self.anchor_tovic_pub_.publish(tubes_cmd)
-        return response
+    def publish_feedback(self):
+        self.feedback_pub.publish(self.bio_feedback)
 
-    def execute_vacuum(self, goal_handle):
-        valve_id = int.from_bytes(goal_handle.request.valve_id)
-        duty = int.from_bytes(goal_handle.request.fan_duty_cycle_percent)
-        total = goal_handle.request.fan_time_ms
+    @staticmethod
+    def list_serial_ports():
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+        # return glob.glob("/dev/tty[A-Za-z]*")
 
-        # open valve
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=40,
-                data=[float(valve_id)],
-            )
-        )
-
-        # set fan duty cycle
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=19,
-                data=[float(duty)],
-            )
-        )
-
-        feedback = BioVacuum.Feedback()
-        start = time.time()
-
-        while True:
-            elapsed = int((time.time() - start) * 1000)
-            remaining = max(0, total - elapsed)
-
-            feedback.fan_time_remaining_ms = remaining
-            goal_handle.publish_feedback(feedback)
-
-            if remaining == 0:
-                break
-
-            time.sleep(0.1)
-
-        # stop fan
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=19,
-                data=[0.0],
-            )
-        )
-
-        # close valve
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=40,
-                data=[-1.0],
-            )
-        )
-
-        goal_handle.succeed()
-        return BioVacuum.Result()
-
-
-def clamp_short(x: int) -> int:
-    return max(-32768, min(32767, x))
+    def cleanup(self):
+        print("Cleaning up...")
+        try:
+            if self.ser.is_open:
+                self.ser.close()
+        except Exception as e:
+            exit(0)
 
 
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
 
 
 def main(args=None):

src/servo_arm_twist_pkg/src/joystick_twist.cpp

@@ -43,16 +43,12 @@
 #include <control_msgs/msg/joint_jog.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #include <moveit_msgs/msg/planning_scene.hpp>
-#include <rclcpp/client.hpp>
-#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
+
 #include <rclcpp/node.hpp>
 #include <rclcpp/publisher.hpp>
-#include <rclcpp/qos.hpp>
-#include <rclcpp/qos_event.hpp>
 #include <rclcpp/subscription.hpp>
+#include <rclcpp/qos.hpp>
 #include <rclcpp/time.hpp>
-#include <rclcpp/utilities.hpp>
-#include <thread>
 
 // We'll just set up parameters here
 const std::string JOY_TOPIC = "/joy";
@@ -104,8 +100,9 @@ std::map<Button, double> BUTTON_DEFAULTS;
  * @return return true if you want to publish a Twist, false if you want to publish a JointJog
  */
 bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
-                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
-                     std::unique_ptr<control_msgs::msg::JointJog>& joint)
+                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist
+                     // std::unique_ptr<control_msgs::msg::JointJog>& joint
+                    )
 {
     // // Give joint jogging priority because it is only buttons
     // // If any joint jog command is requested, we are only publishing joint commands
@@ -236,7 +233,7 @@ public:
         updateCmdFrame(frame_to_publish_, msg->buttons);
 
         // Convert the joystick message to Twist or JointJog and publish
-        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
+        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg))
         {
             // publish the TwistStamped
             twist_msg->header.frame_id = frame_to_publish_;