Fixed README

.dockerignore

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

.env

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

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

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

src/anchor_pkg/launch/rover.launch.py

@@ -1,21 +1,13 @@
 #!/usr/bin/env python3
 
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    OpaqueFunction,
-    Shutdown,
-    IncludeLaunchDescription,
-)
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
 from launch.substitutions import (
     LaunchConfiguration,
     ThisLaunchFileDir,
     PathJoinSubstitution,
 )
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.conditions import IfCondition
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 
 
 # Prevent making __pycache__ directories
@@ -48,10 +40,7 @@ def launch_setup(context, *args, **kwargs):
                 executable="core",  # change as needed
                 name="core",
                 output="both",
-                parameters=[
-                    {"launch_mode": mode},
-                    {"use_ros2_control": LaunchConfiguration("use_ros2_control", default=False)},
-                ],
+                parameters=[{"launch_mode": mode}],
                 on_exit=Shutdown(),
             )
         )
@@ -144,46 +133,4 @@ def generate_launch_description():
         description="Launch mode: arm, core, bio, anchor, or ptz",
     )
 
-    ros2_control_arg = DeclareLaunchArgument(
-        "use_ros2_control",
-        default_value="false",
-        description="Whether to use DiffDriveController for driving instead of direct Twist",
-    )
-
-    rsp = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("core_description"),
-                    "launch",
-                    "robot_state_publisher.launch.py",
-                ]
-            )
-        ),
-        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
-        launch_arguments={("hardware_mode", "physical")},
-    )
-
-    controllers = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("core_description"),
-                    "launch",
-                    "spawn_controllers.launch.py",
-                ]
-            )
-        ),
-        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
-        launch_arguments={("hardware_mode", "physical")},
-    )
-
-    return LaunchDescription(
-        [
-            declare_arg,
-            ros2_control_arg,
-            rsp,
-            controllers,
-            OpaqueFunction(function=launch_setup),
-        ]
-    )
+    return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])

src/core_pkg/core_pkg/core_node.py

@@ -66,10 +66,6 @@ class SerialRelay(Node):
         self.launch_mode = self.get_parameter("launch_mode").value
         self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
 
-        self.declare_parameter("use_ros2_control", False)
-        self.use_ros2_control = self.get_parameter("use_ros2_control").value
-        self.get_logger().info(f"Use ros2_control: {self.use_ros2_control}")
-
         ##################################################
         # Topics
 
@@ -89,28 +85,24 @@ class SerialRelay(Node):
 
         # Control
 
-        if self.use_ros2_control:
-            # Joint state control for topic-based controller
-            self.joint_command_sub_ = self.create_subscription(
-                JointState, "/core/joint_commands", self.joint_command_callback, 2
-            )
-        else:
-            # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-            self.cmd_vel_sub_ = self.create_subscription(
-                TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
-            )
-            # manual twist -- [-1, 1] rather than real units
-            self.twist_man_sub_ = self.create_subscription(
-                Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
-            )
-            # manual flags -- brake mode and max duty cycle
-            self.control_state_sub_ = self.create_subscription(
-                CoreCtrlState,
-                "/core/control/state",
-                self.control_state_callback,
-                qos_profile=control_qos,
-            )
-            self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
+        self.cmd_vel_sub_ = self.create_subscription(
+            TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
+        )
+        # manual twist -- [-1, 1] rather than real units
+        self.twist_man_sub_ = self.create_subscription(
+            Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
+        )
+        # manual flags -- brake mode and max duty cycle
+        self.control_state_sub_ = self.create_subscription(
+            CoreCtrlState,
+            "/core/control/state",
+            self.control_state_callback,
+            qos_profile=control_qos,
+        )
+        self.twist_max_duty = (
+            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        )
 
         # Feedback
 
@@ -130,7 +122,7 @@ class SerialRelay(Node):
         )  # TODO: not sure about this
         # Joint states for topic-based controller
         self.joint_state_pub_ = self.create_publisher(
-            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
+            JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
         )
         # IMU (embedded BNO-055)
         self.imu_pub_ = self.create_publisher(
@@ -156,11 +148,10 @@ class SerialRelay(Node):
 
         # Old
 
-        if not self.use_ros2_control:
-            # /core/control
-            self.control_sub = self.create_subscription(
-                CoreControl, "/core/control", self.send_controls, 10
-            )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
+        # /core/control
+        self.control_sub = self.create_subscription(
+            CoreControl, "/core/control", self.send_controls, 10
+        )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
         # /core/feedback
         self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
         self.core_feedback = CoreFeedback()
@@ -292,40 +283,6 @@ class SerialRelay(Node):
 
         # print(f"[Sys] Relaying: {command}")
 
-    def joint_command_callback(self, msg: JointState):
-        # So... topic based control node publishes JointState messages over /joint_commands
-        #  with len(msg.name) == 5 and len(msg.velocity) == 4... all 5 non-fixed joints
-        #  are included in msg.name, but ig it is implied that msg.velocity only
-        #  includes velocities for the commanded joints (ros__parameters.joints).
-        # So, this will be much more hacky and less adaptable than I would like it to be.
-        if len(msg.name) != 5 or len(msg.velocity) != 4 or len(msg.position) != 0:
-            self.get_logger().warning(
-                f"Received joint control message with unexpected number of joints. Ignoring."
-            )
-            return
-        if msg.name != [
-            "left_suspension_joint",
-            "bl_wheel_joint",
-            "br_wheel_joint",
-            "fl_wheel_joint",
-            "fr_wheel_joint",
-        ]:
-            self.get_logger().warning(
-                f"Received joint control message with unexpected name[]. Ignoring."
-            )
-            return
-
-        (bl_vel, br_vel, fl_vel, fr_vel) = msg.velocity
-
-        bl_rpm = radps_to_rpm(bl_vel) * CORE_GEAR_RATIO
-        br_rpm = radps_to_rpm(br_vel) * CORE_GEAR_RATIO
-        fl_rpm = radps_to_rpm(fl_vel) * CORE_GEAR_RATIO
-        fr_rpm = radps_to_rpm(fr_vel) * CORE_GEAR_RATIO
-
-        self.send_viccan(
-            20, [fl_rpm, bl_rpm, fr_rpm, br_rpm]
-        )  # order expected by embedded
-
     def cmd_vel_callback(self, msg: TwistStamped):
         linear = msg.twist.linear.x
         angular = -msg.twist.angular.z
@@ -571,27 +528,22 @@ class SerialRelay(Node):
                 match motorId:
                     case 1:
                         motor = self.feedback_new_state.fl_motor
-                        joint_state_msg.name = ["fl_wheel_joint"]
+                        joint_state_msg.name = ["fl_motor_joint"]
                     case 2:
                         motor = self.feedback_new_state.bl_motor
-                        joint_state_msg.name = ["bl_wheel_joint"]
+                        joint_state_msg.name = ["bl_motor_joint"]
                     case 3:
                         motor = self.feedback_new_state.fr_motor
-                        joint_state_msg.name = ["fr_wheel_joint"]
+                        joint_state_msg.name = ["fr_motor_joint"]
                     case 4:
                         motor = self.feedback_new_state.br_motor
-                        joint_state_msg.name = ["br_wheel_joint"]
+                        joint_state_msg.name = ["br_motor_joint"]
                     case _:
                         self.get_logger().warning(
                             f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
                         )
                         return
 
-                # make the fucking shit work
-                joint_state_msg.name.append("left_suspension_joint")
-                joint_state_msg.position.append(0.0)
-                joint_state_msg.velocity.append(0.0)
-
                 joint_state_msg.header.stamp = msg.header.stamp
                 self.joint_state_pub_.publish(joint_state_msg)
             case _:
@@ -629,10 +581,6 @@ def map_range(
     return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
 
-def radps_to_rpm(radps: float):
-    return radps * 60 / (2 * pi)
-
-
 def main(args=None):
     rclpy.init(args=args)
     sys.excepthook = myexcepthook

src/headless_pkg/src/headless_node.py

@@ -17,8 +17,7 @@ import grp
 from math import copysign
 
 from std_msgs.msg import String
-from geometry_msgs.msg import Twist, TwistStamped
-from std_msgs.msg import Header
+from geometry_msgs.msg import Twist
 from astra_msgs.msg import CoreControl, ArmManual, BioControl
 from astra_msgs.msg import CoreCtrlState
 
@@ -56,16 +55,6 @@ class Headless(Node):
         pygame.joystick.init()
         super().__init__("headless")
 
-        self.declare_parameter("use_cmd_vel", False)
-        self.using_cmd_vel = self.get_parameter("use_cmd_vel").value
-        if self.using_cmd_vel:
-            self.get_logger().info("Using cmd_vel for core control")
-            global CORE_MODE
-            CORE_MODE = "twist"
-        else:
-            self.get_logger().info("Using astra_msgs/CoreControl for core control")
-
-
         # Wait for anchor to start
         pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
         while len(pub_info) == 0:
@@ -125,9 +114,6 @@ class Headless(Node):
         self.core_twist_pub_ = self.create_publisher(
             Twist, "/core/twist", qos_profile=control_qos
         )
-        self.core_cmd_vel_pub_ = self.create_publisher(
-            TwistStamped, "/diff_controller/cmd_vel", qos_profile=control_qos
-        )
         self.core_state_pub_ = self.create_publisher(
             CoreCtrlState, "/core/control/state", qos_profile=control_qos
         )
@@ -139,9 +125,6 @@ class Headless(Node):
         # Rumble when node is ready (returns False if rumble not supported)
         self.gamepad.rumble(0.7, 0.8, 150)
 
-        # Added so you can tell when it starts running after changing the constant logging to debug from info
-        self.get_logger().info("Defaulting to Core mode. Ready.")
-
     def run(self):
         # This thread makes all the update processes run in the background
         thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
@@ -215,7 +198,7 @@ class Headless(Node):
             # self.bio_publisher.publish(BIO_STOP_MSG)
 
         elif self.ctrl_mode == "core" and CORE_MODE == "twist":
-            twist = Twist()
+            input = Twist()
 
             # Collect controller state
             left_stick_y = deadzone(self.gamepad.get_axis(1))
@@ -225,25 +208,17 @@ class Headless(Node):
             right_bumper = self.gamepad.get_button(5)
 
             # Forward/back and Turn
-            twist.linear.x = -1.0 * left_stick_y
-            twist.angular.z = -1.0 * copysign(
+            input.linear.x = -1.0 * left_stick_y
+            input.angular.z = -1.0 * copysign(
                 right_stick_x**2, right_stick_x
             )  # Exponent for finer control (curve)
 
-            if self.using_cmd_vel:
-                twist.linear.x *= 1.5
-                twist.angular.z *= 0.5
-
             # Publish
-            if not self.using_cmd_vel:
-                self.core_twist_pub_.publish(twist)
-            else:
-                header = Header(stamp=self.get_clock().now().to_msg())
-                self.core_cmd_vel_pub_.publish(TwistStamped(header=header, twist=twist))
+            self.core_twist_pub_.publish(input)
             self.arm_publisher.publish(ARM_STOP_MSG)
             # self.bio_publisher.publish(BIO_STOP_MSG)
-            self.get_logger().debug(
-                f"[Core Ctrl] Linear: {round(twist.linear.x, 2)}, Angular: {round(twist.angular.z, 2)}"
+            self.get_logger().info(
+                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
             )
 
             # Brake mode

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_;