mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-04-20 20:01:15 -05:00
Compare commits
14 Commits
f7efa604d2
...
bio-topic-
| Author | SHA1 | Date | |
|---|---|---|---|
|
24b805d37c | ||
|
|
5a2358a176 | ||
|
|
6061baec1b | ||
|
|
8eba7fd671 | ||
|
|
2dbdae8ade | ||
|
|
bc6c325e4e | ||
|
|
caab0ebd7a | ||
|
|
0ce872d9c9 | ||
|
|
7e6a53b7f1 | ||
|
|
9510270272 | ||
|
|
d4042f8744 | ||
|
|
d46bb81ed1 | ||
|
|
87a764e616 | ||
|
|
f99b9e6f96 |
3
.gitmodules
vendored
3
.gitmodules
vendored
@@ -1,6 +1,3 @@
|
||||
[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
|
||||
|
||||
76
README.md
76
README.md
@@ -16,9 +16,6 @@ You will use these packages to launch all rover-side ROS2 nodes.
|
||||
- [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
|
||||
- [Common Problems/Toubleshooting](#common-problemstroubleshooting)
|
||||
- [Packages](#packages)
|
||||
- [Graphs](#graphs)
|
||||
- [Full System](#full-system)
|
||||
- [Individual Nodes](#individual-nodes)
|
||||
- [Maintainers](#maintainers)
|
||||
|
||||
## Software Prerequisites
|
||||
@@ -60,20 +57,6 @@ $ ros2 launch anchor_pkg rover.launch.py # Must be run on a computer connected
|
||||
$ ros2 run headless_pkg headless_full # Optionally run in a separate shell on the same or different computer.
|
||||
```
|
||||
|
||||
### Using the Mock Connector
|
||||
|
||||
Anchor provides a mock connector meant for testing and scripting purposes. You can select the mock connector by running anchor with this command:
|
||||
|
||||
```bash
|
||||
$ ros2 launch anchor_pkg rover.launch.py connector:="mock"
|
||||
```
|
||||
|
||||
You can see all data sent to it in a string format with this command:
|
||||
|
||||
```bash
|
||||
$ ros2 topic echo /anchor/to_vic/debug
|
||||
```
|
||||
|
||||
### Testing Serial
|
||||
|
||||
You can fake the presence of a Serial device (i.e., MCU) by using the following command:
|
||||
@@ -82,31 +65,10 @@ You can fake the presence of a Serial device (i.e., MCU) by using the following
|
||||
$ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT
|
||||
```
|
||||
|
||||
When you go to run anchor, use the `serial_override` ROS2 parameter to point it to the fake serial port, like so:
|
||||
When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
|
||||
|
||||
```bash
|
||||
$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9
|
||||
```
|
||||
|
||||
### Testing CAN
|
||||
|
||||
You can create a virtual CAN network by using the following commands to create and then enable it:
|
||||
|
||||
```bash
|
||||
sudo ip link add dev vcan0 type vcan
|
||||
sudo ip link set vcan0 up
|
||||
```
|
||||
|
||||
When you go to run anchor, use the `can_override` ROS2 parameter to point it to the virtual CAN network, like so:
|
||||
|
||||
```bash
|
||||
$ ros2 launch anchor_pkg rover.launch.py connector:=can can_override:=vcan0
|
||||
```
|
||||
|
||||
Once you're done, you should delete the virtual network so that anchor doesn't get confused if you plug in a real CAN adapter:
|
||||
|
||||
```bash
|
||||
$ sudo ip link delete vcan0
|
||||
$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
|
||||
```
|
||||
|
||||
### Connecting the GuliKit Controller
|
||||
@@ -178,40 +140,6 @@ A: To find a microcontroller to talk to, Anchor sends a ping to every Serial por
|
||||
- [ros2\_interfaces\_pkg](./src/ros2_interfaces_pkg) - Contains custom message types for communication between basestation and the rover over ROS2. (being renamed to `astra_msgs`).
|
||||
- [servo\_arm\_twist\_pkg](./src/servo_arm_twist_pkg) - A temporary node to translate controller state from `ros2_joy` to `Twist` messages to control the Arm via IK.
|
||||
|
||||
## Graphs
|
||||
|
||||
### Full System
|
||||
|
||||
> **Anchor stand-alone** (`ros2 launch anchor_pkg rover.launch.py`)
|
||||
>
|
||||
> 
|
||||
|
||||
> **Anchor with [basestation-classic](https://github.com/SHC-ASTRA/basestation-classic)**
|
||||
>
|
||||
> 
|
||||
|
||||
> **Anchor with Headless** (`ros2 run headless_pkg headless_full`)
|
||||
>
|
||||
> 
|
||||
|
||||
### Individual Nodes
|
||||
|
||||
> **Anchor** (`ros2 run anchor_pkg anchor`)
|
||||
>
|
||||
> 
|
||||
|
||||
> **Core** (`ros2 run core_pkg core --ros-args -p launch_mode:=anchor`)
|
||||
>
|
||||
> 
|
||||
|
||||
> **Arm** (`ros2 run arm_pkg arm --ros-args -p launch_mode:=anchor`)
|
||||
>
|
||||
> 
|
||||
|
||||
> **Bio** (`ros2 run bio_pkg bio --ros-args -p launch_mode:=anchor`)
|
||||
>
|
||||
> 
|
||||
|
||||
## Maintainers
|
||||
|
||||
| Name | Email | Discord |
|
||||
|
||||
@@ -14,12 +14,8 @@ 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 command -v nixos-rebuild; then
|
||||
echo "[INFO] running on NixOS"
|
||||
else
|
||||
source /opt/ros/humble/setup.bash
|
||||
fi
|
||||
# Source ROS 2 Humble setup script (if we aren't using nix)
|
||||
command -v ros2 || source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source your workspace setup script
|
||||
source $SCRIPT_DIR/../install/setup.bash
|
||||
|
||||
@@ -15,11 +15,7 @@ echo "[INFO] Network interface is up!"
|
||||
echo "[INFO] Starting ROS node..."
|
||||
|
||||
# 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
|
||||
command -v ros2 || source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source your workspace setup script
|
||||
source $SCRIPT_DIR/../install/setup.bash
|
||||
|
||||
@@ -17,11 +17,7 @@ done
|
||||
echo "[INFO] Network interface is up!"
|
||||
|
||||
|
||||
if command -v nixos-rebuild; then
|
||||
echo "[INFO] running on NixOS"
|
||||
else
|
||||
source /opt/ros/humble/setup.bash
|
||||
fi
|
||||
command -v ros2 || source /opt/ros/humble/setup.bash
|
||||
source $ANCHOR_WS/install/setup.bash
|
||||
[[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash
|
||||
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 110 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 98 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 36 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 119 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 395 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 543 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 439 KiB |
111
flake.lock
generated
111
flake.lock
generated
@@ -1,5 +1,29 @@
|
||||
{
|
||||
"nodes": {
|
||||
"astra-msgs": {
|
||||
"inputs": {
|
||||
"nix-ros-overlay": "nix-ros-overlay",
|
||||
"nixpkgs": [
|
||||
"astra-msgs",
|
||||
"nix-ros-overlay",
|
||||
"nixpkgs"
|
||||
]
|
||||
},
|
||||
"locked": {
|
||||
"lastModified": 1771845042,
|
||||
"narHash": "sha256-pGsb93ZlMhP3biy8S2eJc1sW+35vmaxlWHTbuwZDlQI=",
|
||||
"owner": "SHC-ASTRA",
|
||||
"repo": "astra_msgs",
|
||||
"rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
|
||||
"type": "github"
|
||||
},
|
||||
"original": {
|
||||
"owner": "SHC-ASTRA",
|
||||
"repo": "astra_msgs",
|
||||
"rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
|
||||
"type": "github"
|
||||
}
|
||||
},
|
||||
"flake-utils": {
|
||||
"inputs": {
|
||||
"systems": "systems"
|
||||
@@ -18,17 +42,55 @@
|
||||
"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": 1770108954,
|
||||
"narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
|
||||
"lastModified": 1770622967,
|
||||
"narHash": "sha256-1LYjTugPSCa/5NkP6/dcZLH5TQYj3R8mAZ/9dgd7jDM=",
|
||||
"owner": "lopsided98",
|
||||
"repo": "nix-ros-overlay",
|
||||
"rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
|
||||
"rev": "d1b9f17eba909116356436d46b5192d299c6b49a",
|
||||
"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": 1771404745,
|
||||
"narHash": "sha256-UVP3TsQJ4PezyQG3B1SsgsTxz32XBVzplJ/cgq7v/uk=",
|
||||
"owner": "lopsided98",
|
||||
"repo": "nix-ros-overlay",
|
||||
"rev": "7cdf7b44ff186869baedbb3b82e5b409bb3e8dd9",
|
||||
"type": "github"
|
||||
},
|
||||
"original": {
|
||||
@@ -54,14 +116,30 @@
|
||||
"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": {
|
||||
"nix-ros-overlay": "nix-ros-overlay",
|
||||
"astra-msgs": "astra-msgs",
|
||||
"nix-ros-overlay": "nix-ros-overlay_2",
|
||||
"nixpkgs": [
|
||||
"nix-ros-overlay",
|
||||
"nixpkgs"
|
||||
],
|
||||
"treefmt-nix": "treefmt-nix"
|
||||
]
|
||||
}
|
||||
},
|
||||
"systems": {
|
||||
@@ -79,23 +157,18 @@
|
||||
"type": "github"
|
||||
}
|
||||
},
|
||||
"treefmt-nix": {
|
||||
"inputs": {
|
||||
"nixpkgs": [
|
||||
"nixpkgs"
|
||||
]
|
||||
},
|
||||
"systems_2": {
|
||||
"locked": {
|
||||
"lastModified": 1773297127,
|
||||
"narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=",
|
||||
"owner": "numtide",
|
||||
"repo": "treefmt-nix",
|
||||
"rev": "71b125cd05fbfd78cab3e070b73544abe24c5016",
|
||||
"lastModified": 1681028828,
|
||||
"narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
|
||||
"owner": "nix-systems",
|
||||
"repo": "default",
|
||||
"rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
|
||||
"type": "github"
|
||||
},
|
||||
"original": {
|
||||
"owner": "numtide",
|
||||
"repo": "treefmt-nix",
|
||||
"owner": "nix-systems",
|
||||
"repo": "default",
|
||||
"type": "github"
|
||||
}
|
||||
}
|
||||
|
||||
58
flake.nix
58
flake.nix
@@ -3,21 +3,13 @@
|
||||
|
||||
inputs = {
|
||||
nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
|
||||
nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
|
||||
|
||||
treefmt-nix = {
|
||||
url = "github:numtide/treefmt-nix";
|
||||
inputs.nixpkgs.follows = "nixpkgs";
|
||||
};
|
||||
nixpkgs.follows = "nix-ros-overlay/nixpkgs";
|
||||
astra-msgs.url =
|
||||
"github:SHC-ASTRA/astra_msgs/acabfd117d9711afc420612375b4e02f4ce4982d";
|
||||
};
|
||||
|
||||
outputs =
|
||||
{
|
||||
self,
|
||||
nix-ros-overlay,
|
||||
nixpkgs,
|
||||
...
|
||||
}@inputs:
|
||||
{ self, nix-ros-overlay, nixpkgs, astra-msgs }:
|
||||
nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
|
||||
system:
|
||||
let
|
||||
@@ -25,27 +17,31 @@
|
||||
inherit system;
|
||||
overlays = [ nix-ros-overlay.overlays.default ];
|
||||
};
|
||||
|
||||
astra_msgs_pkgs = astra-msgs.packages.${system};
|
||||
rosDistro = "humble";
|
||||
in
|
||||
{
|
||||
devShells.default = pkgs.mkShell {
|
||||
name = "ASTRA Anchor";
|
||||
|
||||
packages = with pkgs; [
|
||||
colcon
|
||||
(python313.withPackages (
|
||||
p: with p; [
|
||||
pyserial
|
||||
python-can
|
||||
pygame
|
||||
scipy
|
||||
crccheck
|
||||
black
|
||||
]
|
||||
))
|
||||
(
|
||||
with rosPackages.humble;
|
||||
astra_msgs_pkgs.astra-msgs
|
||||
|
||||
(pkgs.rosPackages.${rosDistro}.python3.withPackages (p: with p; [
|
||||
pyserial
|
||||
pygame
|
||||
scipy
|
||||
crccheck
|
||||
black
|
||||
]))
|
||||
|
||||
(with rosPackages.${rosDistro};
|
||||
buildEnv {
|
||||
paths = [
|
||||
ros-core
|
||||
rqt-graph
|
||||
ros2cli
|
||||
ros2run
|
||||
ros2bag
|
||||
@@ -81,22 +77,24 @@
|
||||
ros2-controllers
|
||||
chomp-motion-planner
|
||||
];
|
||||
}
|
||||
)
|
||||
})
|
||||
];
|
||||
|
||||
env = {
|
||||
ASTRAMSGS = "${astra-msgs.outPath}";
|
||||
};
|
||||
|
||||
shellHook = ''
|
||||
# Display stuff
|
||||
export DISPLAY=''${DISPLAY:-:0}
|
||||
export QT_X11_NO_MITSHM=1
|
||||
'';
|
||||
};
|
||||
|
||||
formatter = (inputs.treefmt-nix.lib.evalModule pkgs ./treefmt.nix).config.build.wrapper;
|
||||
}
|
||||
);
|
||||
|
||||
nixConfig = {
|
||||
extra-substituters = [ "https://ros.cachix.org" ];
|
||||
extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
|
||||
extra-trusted-public-keys =
|
||||
[ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
|
||||
};
|
||||
}
|
||||
|
||||
@@ -1,24 +1,28 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rcl_interfaces.msg import ParameterDescriptor, ParameterType
|
||||
from std_srvs.srv import Empty
|
||||
|
||||
import signal
|
||||
import time
|
||||
import atexit
|
||||
|
||||
from .connector import (
|
||||
Connector,
|
||||
MockConnector,
|
||||
SerialConnector,
|
||||
CANConnector,
|
||||
NoValidDeviceException,
|
||||
NoWorkingDeviceException,
|
||||
)
|
||||
from .convert import string_to_viccan, viccan_to_string
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import glob
|
||||
|
||||
from std_msgs.msg import String, Header
|
||||
from astra_msgs.msg import VicCAN
|
||||
from std_msgs.msg import String
|
||||
|
||||
KNOWN_USBS = [
|
||||
(0x2E8A, 0x00C0), # Raspberry Pi Pico
|
||||
(0x1A86, 0x55D4), # Adafruit Feather ESP32 V2
|
||||
(0x10C4, 0xEA60), # DOIT ESP32 Devkit V1
|
||||
(0x1A86, 0x55D3), # ESP32 S3 Development Board
|
||||
]
|
||||
|
||||
|
||||
class Anchor(Node):
|
||||
@@ -32,8 +36,6 @@ class Anchor(Node):
|
||||
- VicCAN messages for Arm node
|
||||
* /anchor/from_vic/bio
|
||||
- VicCAN messages for Bio node
|
||||
* /anchor/to_vic/debug
|
||||
- A string copy of the messages published to ./relay are published here
|
||||
|
||||
Subscribers:
|
||||
* /anchor/from_vic/mock_mcu
|
||||
@@ -41,181 +43,301 @@ class Anchor(Node):
|
||||
* /anchor/to_vic/relay
|
||||
- Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
|
||||
* /anchor/to_vic/relay_string
|
||||
- Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
|
||||
- Publish raw strings to this topic to send directly to the MCU for debugging
|
||||
"""
|
||||
|
||||
connector: Connector
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("anchor_node")
|
||||
# Initalize node with name
|
||||
super().__init__("anchor_node") # previously 'serial_publisher'
|
||||
|
||||
logger = self.get_logger()
|
||||
self.serial_port: str | None = None # e.g., "/dev/ttyUSB0"
|
||||
|
||||
# ROS2 Parameter Setup
|
||||
# Serial port override
|
||||
if port_override := os.getenv("PORT_OVERRIDE"):
|
||||
self.serial_port = port_override
|
||||
|
||||
self.declare_parameter(
|
||||
"connector",
|
||||
"auto",
|
||||
ParameterDescriptor(
|
||||
name="connector",
|
||||
description="Declares which MCU connector should be used. Defaults to 'auto'.",
|
||||
type=ParameterType.PARAMETER_STRING,
|
||||
additional_constraints="Must be 'serial', 'can', 'mock', or 'auto'.",
|
||||
),
|
||||
)
|
||||
##################################################
|
||||
# Serial MCU Discovery
|
||||
|
||||
self.declare_parameter(
|
||||
"can_override",
|
||||
"",
|
||||
ParameterDescriptor(
|
||||
name="can_override",
|
||||
description="Overrides which CAN channel will be used. Defaults to ''.",
|
||||
type=ParameterType.PARAMETER_STRING,
|
||||
additional_constraints="Must be a valid CAN network that shows up in `ip link show`.",
|
||||
),
|
||||
)
|
||||
|
||||
self.declare_parameter(
|
||||
"serial_override",
|
||||
"",
|
||||
ParameterDescriptor(
|
||||
name="serial_override",
|
||||
description="Overrides which serial port will be used. Defaults to ''.",
|
||||
type=ParameterType.PARAMETER_STRING,
|
||||
additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.",
|
||||
),
|
||||
)
|
||||
|
||||
# Determine which connector to use. Options are Mock, Serial, and CAN
|
||||
connector_select = (
|
||||
self.get_parameter("connector").get_parameter_value().string_value
|
||||
)
|
||||
can_override = (
|
||||
self.get_parameter("can_override").get_parameter_value().string_value
|
||||
)
|
||||
serial_override = (
|
||||
self.get_parameter("serial_override").get_parameter_value().string_value
|
||||
)
|
||||
match connector_select:
|
||||
case "serial":
|
||||
logger.info("using serial connector")
|
||||
self.connector = SerialConnector(
|
||||
logger, self.get_clock(), serial_override
|
||||
# If there was not a port override, look for a MCU over USB for Serial.
|
||||
if self.serial_port is None:
|
||||
comports = serial.tools.list_ports.comports()
|
||||
real_ports = list(
|
||||
filter(
|
||||
lambda p: p.vid is not None
|
||||
and p.pid is not None
|
||||
and p.device is not None,
|
||||
comports,
|
||||
)
|
||||
case "can":
|
||||
logger.info("using CAN connector")
|
||||
self.connector = CANConnector(logger, self.get_clock(), can_override)
|
||||
case "mock":
|
||||
logger.info("using mock connector")
|
||||
self.connector = MockConnector(logger, self.get_clock())
|
||||
case "auto":
|
||||
logger.info("automatically determining connector")
|
||||
try:
|
||||
logger.info("trying CAN connector")
|
||||
self.connector = CANConnector(
|
||||
logger, self.get_clock(), can_override
|
||||
)
|
||||
except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
|
||||
logger.info("CAN connector failed, trying serial connector")
|
||||
self.connector = SerialConnector(
|
||||
logger, self.get_clock(), serial_override
|
||||
)
|
||||
case _:
|
||||
logger.fatal(
|
||||
f"invalid value for connector parameter: {connector_select}"
|
||||
)
|
||||
recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
|
||||
|
||||
if len(recog_ports) == 1: # Found singular recognized MCU
|
||||
found_port = recog_ports[0]
|
||||
self.get_logger().info(
|
||||
f"Selecting MCU '{found_port.description}' at {found_port.device}."
|
||||
)
|
||||
exit(1)
|
||||
self.serial_port = found_port.device # String, location of device file; e.g., '/dev/ttyACM0'
|
||||
elif len(recog_ports) > 1: # Found multiple recognized MCUs
|
||||
# Kinda jank log message
|
||||
self.get_logger().error(
|
||||
f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
|
||||
)
|
||||
# Don't set self.serial_port; later if-statement will exit()
|
||||
elif (
|
||||
len(recog_ports) == 0 and len(real_ports) > 0
|
||||
): # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
|
||||
self.get_logger().error(
|
||||
f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
|
||||
)
|
||||
# Don't set self.serial_port; later if-statement will exit()
|
||||
else: # Found jack shit
|
||||
self.get_logger().error("No valid Serial ports specified or found.")
|
||||
# Don't set self.serial_port; later if-statement will exit()
|
||||
|
||||
# ROS2 Topic Setup
|
||||
# We still don't have a serial port; fall back to legacy discovery (Areeb's code)
|
||||
# Loop through all serial devices on the computer to check for the MCU
|
||||
if self.serial_port is None:
|
||||
self.get_logger().warning("Falling back to legacy MCU discovery...")
|
||||
ports = Anchor.list_serial_ports()
|
||||
for _ in range(4):
|
||||
if self.serial_port is not None:
|
||||
break
|
||||
for port in ports:
|
||||
try:
|
||||
# connect and send a ping command
|
||||
ser = serial.Serial(port, 115200, timeout=1)
|
||||
# (f"Checking port {port}...")
|
||||
ser.write(b"ping\n")
|
||||
response = ser.read_until(bytes("\n", "utf8"))
|
||||
|
||||
# Publishers
|
||||
self.fromvic_debug_pub_ = self.create_publisher( # only used by serial
|
||||
String,
|
||||
"/anchor/from_vic/debug",
|
||||
20,
|
||||
)
|
||||
self.fromvic_core_pub_ = self.create_publisher(
|
||||
VicCAN,
|
||||
"/anchor/from_vic/core",
|
||||
20,
|
||||
)
|
||||
self.fromvic_arm_pub_ = self.create_publisher(
|
||||
VicCAN,
|
||||
"/anchor/from_vic/arm",
|
||||
20,
|
||||
)
|
||||
self.fromvic_bio_pub_ = self.create_publisher(
|
||||
VicCAN,
|
||||
"/anchor/from_vic/bio",
|
||||
20,
|
||||
)
|
||||
# Debug publisher
|
||||
self.tovic_debug_pub_ = self.create_publisher(
|
||||
VicCAN,
|
||||
"/anchor/to_vic/debug",
|
||||
20,
|
||||
)
|
||||
# if pong is in response, then we are talking with the MCU
|
||||
if b"pong" in response:
|
||||
self.serial_port = port
|
||||
self.get_logger().info(f"Found MCU at {self.serial_port}!")
|
||||
break
|
||||
except:
|
||||
pass
|
||||
|
||||
# Subscribers
|
||||
self.tovic_sub_ = self.create_subscription(
|
||||
VicCAN,
|
||||
"/anchor/to_vic/relay",
|
||||
self.write_connector,
|
||||
20,
|
||||
)
|
||||
self.mock_mcu_sub_ = self.create_subscription(
|
||||
String,
|
||||
"/anchor/from_vic/mock_mcu",
|
||||
self.on_mock_fromvic,
|
||||
20,
|
||||
)
|
||||
self.tovic_string_sub_ = self.create_subscription(
|
||||
String,
|
||||
"/anchor/to_vic/relay_string",
|
||||
self.connector.write_raw,
|
||||
20,
|
||||
)
|
||||
# If port is still None then we ain't finding no mcu
|
||||
if self.serial_port is None:
|
||||
self.get_logger().error("Unable to find MCU. Exiting...")
|
||||
time.sleep(1)
|
||||
sys.exit(1)
|
||||
# Found a Serial port, try to open it; above code has not officially opened a Serial port
|
||||
else:
|
||||
self.get_logger().debug(
|
||||
f"Attempting to open Serial port '{self.serial_port}'..."
|
||||
)
|
||||
try:
|
||||
self.serial_interface = serial.Serial(
|
||||
self.serial_port, 115200, timeout=1
|
||||
)
|
||||
|
||||
# Close devices on exit
|
||||
# Attempt to get name of connected MCU
|
||||
self.serial_interface.write(
|
||||
b"can_relay_mode,on\n"
|
||||
) # can_relay_ready,[mcu]
|
||||
mcu_name: str = ""
|
||||
for _ in range(4):
|
||||
response = self.serial_interface.read_until(bytes("\n", "utf8"))
|
||||
try:
|
||||
if b"can_relay_ready" in response:
|
||||
args: list[str] = response.decode("utf8").strip().split(",")
|
||||
if len(args) == 2:
|
||||
mcu_name = args[1]
|
||||
break
|
||||
except UnicodeDecodeError:
|
||||
pass # ignore malformed responses
|
||||
self.get_logger().info(
|
||||
f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
|
||||
)
|
||||
|
||||
except serial.SerialException as e:
|
||||
self.get_logger().error(
|
||||
f"Could not open Serial port '{self.serial_port}' for reason:"
|
||||
)
|
||||
self.get_logger().error(e.strerror)
|
||||
time.sleep(1)
|
||||
sys.exit(1)
|
||||
|
||||
# Close serial port on exit
|
||||
atexit.register(self.cleanup)
|
||||
|
||||
def cleanup(self):
|
||||
self.connector.cleanup()
|
||||
##################################################
|
||||
# ROS2 Topic Setup
|
||||
|
||||
def read_connector(self):
|
||||
"""Check the connector for new data from the MCU, and publish string to appropriate topics"""
|
||||
viccan, raw = self.connector.read()
|
||||
# New pub/sub with VicCAN
|
||||
self.fromvic_debug_pub_ = self.create_publisher(
|
||||
String, "/anchor/from_vic/debug", 20
|
||||
)
|
||||
self.fromvic_core_pub_ = self.create_publisher(
|
||||
VicCAN, "/anchor/from_vic/core", 20
|
||||
)
|
||||
self.fromvic_arm_pub_ = self.create_publisher(
|
||||
VicCAN, "/anchor/from_vic/arm", 20
|
||||
)
|
||||
self.fromvic_bio_pub_ = self.create_publisher(
|
||||
VicCAN, "/anchor/from_vic/bio", 20
|
||||
)
|
||||
|
||||
if raw:
|
||||
self.fromvic_debug_pub_.publish(String(data=raw))
|
||||
self.mock_mcu_sub_ = self.create_subscription(
|
||||
String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
|
||||
)
|
||||
self.tovic_sub_ = self.create_subscription(
|
||||
VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
|
||||
)
|
||||
self.tovic_debug_sub_ = self.create_subscription(
|
||||
String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
|
||||
)
|
||||
|
||||
if viccan:
|
||||
self.relay_fromvic(viccan)
|
||||
# Create publishers
|
||||
self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
|
||||
self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
|
||||
self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
|
||||
|
||||
def write_connector(self, msg: VicCAN):
|
||||
"""Write to the connector and send a copy to /anchor/to_vic/debug"""
|
||||
self.connector.write(msg)
|
||||
self.tovic_debug_pub_.publish(viccan_to_string(msg))
|
||||
self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
|
||||
|
||||
def relay_fromvic(self, msg: VicCAN):
|
||||
"""Relay a message from the MCU to the appropriate VicCAN topic"""
|
||||
if msg.mcu_name == "core":
|
||||
self.fromvic_core_pub_.publish(msg)
|
||||
elif msg.mcu_name == "arm" or msg.mcu_name == "digit":
|
||||
self.fromvic_arm_pub_.publish(msg)
|
||||
elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
|
||||
self.fromvic_bio_pub_.publish(msg)
|
||||
# Create a subscriber
|
||||
self.relay_sub = self.create_subscription(
|
||||
String, "/anchor/relay", self.on_relay_tovic_string, 10
|
||||
)
|
||||
|
||||
def read_MCU(self):
|
||||
"""Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
|
||||
try:
|
||||
output = str(self.serial_interface.readline(), "utf8")
|
||||
|
||||
if output:
|
||||
self.relay_fromvic(output)
|
||||
# All output over debug temporarily
|
||||
# self.get_logger().info(f"[MCU] {output}")
|
||||
msg = String()
|
||||
msg.data = output
|
||||
self.debug_pub.publish(msg)
|
||||
if output.startswith("can_relay_fromvic,core"):
|
||||
self.core_pub.publish(msg)
|
||||
elif output.startswith("can_relay_fromvic,arm") or output.startswith(
|
||||
"can_relay_fromvic,digit"
|
||||
): # digit for voltage readings
|
||||
self.arm_pub.publish(msg)
|
||||
if output.startswith("can_relay_fromvic,citadel") or output.startswith(
|
||||
"can_relay_fromvic,digit"
|
||||
): # digit for SHT sensor
|
||||
self.bio_pub.publish(msg)
|
||||
# msg = String()
|
||||
# msg.data = output
|
||||
# self.debug_pub.publish(msg)
|
||||
return
|
||||
except serial.SerialException as e:
|
||||
print(f"SerialException: {e}")
|
||||
print("Closing serial port.")
|
||||
try:
|
||||
if self.serial_interface.is_open:
|
||||
self.serial_interface.close()
|
||||
except:
|
||||
pass
|
||||
exit(1)
|
||||
except TypeError as e:
|
||||
print(f"TypeError: {e}")
|
||||
print("Closing serial port.")
|
||||
try:
|
||||
if self.serial_interface.is_open:
|
||||
self.serial_interface.close()
|
||||
except:
|
||||
pass
|
||||
exit(1)
|
||||
except Exception as e:
|
||||
print(f"Exception: {e}")
|
||||
# print("Closing serial port.")
|
||||
# if self.ser.is_open:
|
||||
# self.ser.close()
|
||||
# exit(1)
|
||||
|
||||
def on_mock_fromvic(self, msg: String):
|
||||
"""Relay a message as if it came from the MCU"""
|
||||
viccan = string_to_viccan(
|
||||
msg.data,
|
||||
"mock",
|
||||
self.get_logger(),
|
||||
self.get_clock().now().to_msg(),
|
||||
"""For testing without an actual MCU, publish strings here as if they came from an MCU"""
|
||||
# self.get_logger().info(f"Got command from mock MCU: {msg}")
|
||||
self.relay_fromvic(msg.data)
|
||||
|
||||
def on_relay_tovic_viccan(self, msg: VicCAN):
|
||||
"""Relay a VicCAN message to the MCU"""
|
||||
output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
|
||||
for num in msg.data:
|
||||
output += f",{round(num, 7)}" # limit to 7 decimal places
|
||||
output += "\n"
|
||||
# self.get_logger().info(f"VicCAN relay to MCU: {output}")
|
||||
self.serial_interface.write(bytes(output, "utf8"))
|
||||
|
||||
def relay_fromvic(self, msg: str):
|
||||
"""Relay a string message from the MCU to the appropriate VicCAN topic"""
|
||||
self.fromvic_debug_pub_.publish(String(data=msg))
|
||||
parts = msg.strip().split(",")
|
||||
if len(parts) > 0 and parts[0] != "can_relay_fromvic":
|
||||
self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
|
||||
return
|
||||
|
||||
# String validation
|
||||
malformed: bool = False
|
||||
malformed_reason: str = ""
|
||||
if len(parts) < 3 or len(parts) > 7:
|
||||
malformed = True
|
||||
malformed_reason = (
|
||||
f"invalid argument count (expected [3,7], got {len(parts)})"
|
||||
)
|
||||
elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
|
||||
malformed = True
|
||||
malformed_reason = f"invalid mcu_name '{parts[1]}'"
|
||||
elif not (parts[2].isnumeric()) or int(parts[2]) < 0:
|
||||
malformed = True
|
||||
malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
|
||||
else:
|
||||
for x in parts[3:]:
|
||||
try:
|
||||
float(x)
|
||||
except ValueError:
|
||||
malformed = True
|
||||
malformed_reason = f"data '{x}' is not a float"
|
||||
break
|
||||
|
||||
if malformed:
|
||||
self.get_logger().warning(
|
||||
f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
|
||||
)
|
||||
return
|
||||
|
||||
# Have valid VicCAN message
|
||||
|
||||
output = VicCAN()
|
||||
output.mcu_name = parts[1]
|
||||
output.command_id = int(parts[2])
|
||||
if len(parts) > 3:
|
||||
output.data = [float(x) for x in parts[3:]]
|
||||
output.header = Header(
|
||||
stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
|
||||
)
|
||||
if viccan:
|
||||
self.relay_fromvic(viccan)
|
||||
|
||||
# self.get_logger().info(f"Relaying from MCU: {output}")
|
||||
if output.mcu_name == "core":
|
||||
self.fromvic_core_pub_.publish(output)
|
||||
elif output.mcu_name == "arm" or output.mcu_name == "digit":
|
||||
self.fromvic_arm_pub_.publish(output)
|
||||
elif output.mcu_name == "citadel" or output.mcu_name == "digit":
|
||||
self.fromvic_bio_pub_.publish(output)
|
||||
|
||||
def on_relay_tovic_string(self, msg: String):
|
||||
"""Relay a raw string message to the MCU for debugging"""
|
||||
message = msg.data
|
||||
# self.get_logger().info(f"Sending command to MCU: {msg}")
|
||||
self.serial_interface.write(bytes(message, "utf8"))
|
||||
|
||||
@staticmethod
|
||||
def list_serial_ports():
|
||||
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
|
||||
|
||||
def cleanup(self):
|
||||
print("Cleaning up before terminating...")
|
||||
if self.serial_interface.is_open:
|
||||
self.serial_interface.close()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
@@ -228,9 +350,16 @@ def main(args=None):
|
||||
|
||||
rate = anchor_node.create_rate(100) # 100 Hz -- arbitrary rate
|
||||
while rclpy.ok():
|
||||
anchor_node.read_connector() # Check the connector for updates
|
||||
anchor_node.read_MCU() # Check the MCU for updates
|
||||
rate.sleep()
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
print("Caught shutdown signal, shutting down...")
|
||||
finally:
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
signal.signal(
|
||||
signal.SIGTERM, lambda signum, frame: sys.exit(0)
|
||||
) # Catch termination signals and exit cleanly
|
||||
main()
|
||||
|
||||
@@ -1,472 +0,0 @@
|
||||
from abc import ABC, abstractmethod
|
||||
from astra_msgs.msg import VicCAN
|
||||
from rclpy.clock import Clock
|
||||
from rclpy.impl.rcutils_logger import RcutilsLogger
|
||||
from .convert import string_to_viccan as _string_to_viccan, viccan_to_string
|
||||
|
||||
# CAN
|
||||
import can
|
||||
import can.interfaces.socketcan
|
||||
import struct
|
||||
|
||||
# Serial
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
|
||||
KNOWN_USBS = [
|
||||
(0x2E8A, 0x00C0), # Raspberry Pi Pico
|
||||
(0x1A86, 0x55D4), # Adafruit Feather ESP32 V2
|
||||
(0x10C4, 0xEA60), # DOIT ESP32 Devkit V1
|
||||
(0x1A86, 0x55D3), # ESP32 S3 Development Board
|
||||
]
|
||||
BAUD_RATE = 115200
|
||||
|
||||
|
||||
class NoValidDeviceException(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class NoWorkingDeviceException(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class MultipleValidDevicesException(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class DeviceClosedException(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class Connector(ABC):
|
||||
logger: RcutilsLogger
|
||||
clock: Clock
|
||||
|
||||
def string_to_viccan(self, msg: str, mcu_name: str):
|
||||
"""function currying so that we do not need to pass logger and clock every time"""
|
||||
return _string_to_viccan(
|
||||
msg,
|
||||
mcu_name,
|
||||
self.logger,
|
||||
self.clock.now().to_msg(),
|
||||
)
|
||||
|
||||
def __init__(self, logger: RcutilsLogger, clock: Clock):
|
||||
self.logger = logger
|
||||
self.clock = clock
|
||||
|
||||
@abstractmethod
|
||||
def read(self) -> tuple[VicCAN | None, str | None]:
|
||||
"""
|
||||
Must return a tuple of (VicCAN, debug message or string repr of VicCAN)
|
||||
"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def write(self, msg: VicCAN):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def write_raw(self, msg: str):
|
||||
pass
|
||||
|
||||
def cleanup(self):
|
||||
pass
|
||||
|
||||
|
||||
class SerialConnector(Connector):
|
||||
port: str
|
||||
mcu_name: str
|
||||
serial_interface: serial.Serial
|
||||
|
||||
def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""):
|
||||
super().__init__(logger, clock)
|
||||
|
||||
ports = self._find_ports()
|
||||
mcu_name: str | None = None
|
||||
|
||||
if serial_override:
|
||||
logger.warn(
|
||||
f"using serial_override: `{serial_override}`! this will bypass several checks."
|
||||
)
|
||||
ports = [serial_override]
|
||||
mcu_name = "override"
|
||||
|
||||
if len(ports) <= 0:
|
||||
raise NoValidDeviceException("no valid serial device found")
|
||||
if (l := len(ports)) > 1:
|
||||
raise MultipleValidDevicesException(
|
||||
f"too many ({l}) valid serial devices found"
|
||||
)
|
||||
|
||||
# check each of our ports to make sure one of them is responding
|
||||
port = ports[0]
|
||||
# we might already have a name by now if we overrode earlier
|
||||
mcu_name = mcu_name or self._get_name(port)
|
||||
if not mcu_name:
|
||||
raise NoWorkingDeviceException(
|
||||
f"found {port}, but it did not respond with its name"
|
||||
)
|
||||
|
||||
self.port = port
|
||||
self.mcu_name = mcu_name
|
||||
|
||||
# if we fail at this point, it should crash because we've already tested the port
|
||||
self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=1)
|
||||
|
||||
def _find_ports(self) -> list[str]:
|
||||
"""
|
||||
Finds all valid ports but does not test them
|
||||
|
||||
returns: all valid ports
|
||||
"""
|
||||
comports = serial.tools.list_ports.comports()
|
||||
valid_ports = list(
|
||||
map( # get just device strings
|
||||
lambda p: p.device,
|
||||
filter( # make sure we have a known device
|
||||
lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device is not None,
|
||||
comports,
|
||||
),
|
||||
)
|
||||
)
|
||||
self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
|
||||
return valid_ports
|
||||
|
||||
def _get_name(self, port: str) -> str | None:
|
||||
"""
|
||||
Get the name of the MCU (if it works)
|
||||
|
||||
returns: str name of the MCU, None if it doesn't work
|
||||
"""
|
||||
# attempt to open the serial port
|
||||
serial_interface: serial.Serial
|
||||
try:
|
||||
self.logger.info(f"asking {port} for its name")
|
||||
serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
|
||||
|
||||
serial_interface.write(b"can_relay_mode,on\n")
|
||||
|
||||
for i in range(4):
|
||||
self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name")
|
||||
response = serial_interface.read_until(bytes("\n", "utf8"))
|
||||
try:
|
||||
if b"can_relay_ready" in response:
|
||||
args: list[str] = response.decode("utf8").strip().split(",")
|
||||
if len(args) == 2:
|
||||
self.logger.info(f"we are talking to {args[1]}")
|
||||
return args[1]
|
||||
break
|
||||
except UnicodeDecodeError as e:
|
||||
self.logger.info(
|
||||
f"ignoring UnicodeDecodeError when asking for MCU name: {e}"
|
||||
)
|
||||
|
||||
if serial_interface.is_open:
|
||||
# turn relay mode off if it failed to respond with its name
|
||||
serial_interface.write(b"can_relay_mode,off\n")
|
||||
serial_interface.close()
|
||||
except serial.SerialException as e:
|
||||
self.logger.error(f"SerialException when asking for MCU name: {e}")
|
||||
|
||||
return None
|
||||
|
||||
def read(self) -> tuple[VicCAN | None, str | None]:
|
||||
try:
|
||||
raw = str(self.serial_interface.readline(), "utf8")
|
||||
|
||||
if not raw:
|
||||
return (None, None)
|
||||
|
||||
return (
|
||||
self.string_to_viccan(raw, self.mcu_name),
|
||||
raw,
|
||||
)
|
||||
except serial.SerialException as e:
|
||||
self.logger.error(f"SerialException: {e}")
|
||||
raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
|
||||
except Exception:
|
||||
return (None, None) # pretty much no other error matters
|
||||
|
||||
def write(self, msg: VicCAN):
|
||||
self.write_raw(viccan_to_string(msg))
|
||||
|
||||
def write_raw(self, msg: str):
|
||||
self.serial_interface.write(bytes(msg, "utf8"))
|
||||
|
||||
def cleanup(self):
|
||||
self.logger.info(f"closing serial port if open {self.port}")
|
||||
try:
|
||||
if self.serial_interface.is_open:
|
||||
self.serial_interface.close()
|
||||
except Exception as e:
|
||||
self.logger.error(e)
|
||||
|
||||
|
||||
class CANConnector(Connector):
|
||||
def __init__(self, logger: RcutilsLogger, clock: Clock, can_override: str):
|
||||
super().__init__(logger, clock)
|
||||
|
||||
self.can_channel: str | None = None
|
||||
self.can_bus: can.BusABC | None = None
|
||||
|
||||
avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs()
|
||||
|
||||
if len(avail) == 0:
|
||||
raise NoValidDeviceException("no CAN interfaces found")
|
||||
|
||||
# filter to busses whose channel matches the can_override
|
||||
if can_override:
|
||||
avail = list(
|
||||
filter(
|
||||
lambda b: b.get("channel") == can_override,
|
||||
avail,
|
||||
)
|
||||
)
|
||||
|
||||
if (l := len(avail)) > 1:
|
||||
channels = ", ".join(str(b.get("channel")) for b in avail)
|
||||
raise MultipleValidDevicesException(
|
||||
f"too many ({l}) CAN interfaces found: [{channels}]"
|
||||
)
|
||||
|
||||
bus = avail[0]
|
||||
self.can_channel = str(bus.get("channel"))
|
||||
self.logger.info(f"found CAN interface '{self.can_channel}'")
|
||||
|
||||
try:
|
||||
self.can_bus = can.Bus(
|
||||
interface="socketcan",
|
||||
channel=self.can_channel,
|
||||
bitrate=1_000_000,
|
||||
)
|
||||
except can.CanError as e:
|
||||
raise NoWorkingDeviceException(
|
||||
f"could not open CAN channel '{self.can_channel}': {e}"
|
||||
)
|
||||
|
||||
if self.can_channel and self.can_channel.startswith("v"):
|
||||
self.logger.warn("likely using virtual CAN interface")
|
||||
|
||||
def read(self) -> tuple[VicCAN | None, str | None]:
|
||||
if not self.can_bus:
|
||||
raise DeviceClosedException("CAN bus not initialized")
|
||||
|
||||
try:
|
||||
message = self.can_bus.recv(timeout=0.0)
|
||||
except can.CanError as e:
|
||||
self.logger.error(f"CAN error while receiving: {e}")
|
||||
raise DeviceClosedException("CAN bus closed unexpectedly")
|
||||
|
||||
if message is None:
|
||||
return (None, None)
|
||||
|
||||
arbitration_id = message.arbitration_id & 0x7FF
|
||||
data_bytes = bytes(message.data)
|
||||
|
||||
mcu_key = (arbitration_id >> 8) & 0b111
|
||||
data_type_key = (arbitration_id >> 6) & 0b11
|
||||
command = arbitration_id & 0x3F
|
||||
|
||||
key_to_mcu: dict[int, str] = {
|
||||
1: "broadcast",
|
||||
2: "core",
|
||||
3: "arm",
|
||||
4: "digit",
|
||||
5: "faerie",
|
||||
6: "citadel",
|
||||
}
|
||||
|
||||
mcu_name = key_to_mcu.get(mcu_key)
|
||||
if mcu_name is None:
|
||||
self.logger.warn(
|
||||
f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}"
|
||||
)
|
||||
return (None, None)
|
||||
|
||||
data: list[float] = []
|
||||
|
||||
try:
|
||||
if data_type_key == 3:
|
||||
data = []
|
||||
elif data_type_key == 0:
|
||||
if len(data_bytes) < 8:
|
||||
self.logger.warn(
|
||||
f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
|
||||
)
|
||||
return (None, None)
|
||||
(value,) = struct.unpack("<d", data_bytes[:8])
|
||||
data = [float(value)]
|
||||
elif data_type_key == 1:
|
||||
if len(data_bytes) < 8:
|
||||
self.logger.warn(
|
||||
f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame"
|
||||
)
|
||||
return (None, None)
|
||||
v1, v2 = struct.unpack("<ff", data_bytes[:8])
|
||||
data = [float(v1), float(v2)]
|
||||
elif data_type_key == 2:
|
||||
if len(data_bytes) < 8:
|
||||
self.logger.warn(
|
||||
f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame"
|
||||
)
|
||||
return (None, None)
|
||||
i1, i2, i3, i4 = struct.unpack("<hhhh", data_bytes[:8])
|
||||
data = [float(i1), float(i2), float(i3), float(i4)]
|
||||
else:
|
||||
self.logger.warn(
|
||||
f"received CAN frame with unknown data_type_key {data_type_key}; id=0x{arbitration_id:X}"
|
||||
)
|
||||
return (None, None)
|
||||
except struct.error as e:
|
||||
self.logger.error(f"error unpacking CAN payload: {e}")
|
||||
return (None, None)
|
||||
|
||||
viccan = VicCAN(
|
||||
mcu_name=mcu_name,
|
||||
command_id=int(command),
|
||||
data=data,
|
||||
)
|
||||
|
||||
self.logger.debug(
|
||||
f"received CAN frame id=0x{message.arbitration_id:X}, "
|
||||
f"decoded as VicCAN(mcu_name={viccan.mcu_name}, command_id={viccan.command_id}, data={viccan.data})"
|
||||
)
|
||||
|
||||
return (
|
||||
viccan,
|
||||
f"{viccan.mcu_name},{viccan.command_id},"
|
||||
+ ",".join(map(str, list(viccan.data))),
|
||||
)
|
||||
|
||||
def write(self, msg: VicCAN):
|
||||
if not self.can_bus:
|
||||
raise DeviceClosedException("CAN bus not initialized")
|
||||
|
||||
# Build 11-bit arbitration ID according to the VicCAN scheme:
|
||||
# bits 10..8: targeted MCU key
|
||||
# bits 7..6: data type key
|
||||
# bits 5..0: command
|
||||
|
||||
# Map MCU name to 3-bit key.
|
||||
mcu_name = (msg.mcu_name or "").lower()
|
||||
mcu_key_map: dict[str, int] = {
|
||||
"broadcast": 1,
|
||||
"core": 2,
|
||||
"arm": 3,
|
||||
"digit": 4,
|
||||
"faerie": 5,
|
||||
"citadel": 6,
|
||||
}
|
||||
|
||||
if mcu_name not in mcu_key_map:
|
||||
self.logger.error(
|
||||
f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message"
|
||||
)
|
||||
return
|
||||
|
||||
mcu_key = mcu_key_map[mcu_name] & 0b111
|
||||
|
||||
# Infer data type key from payload length according to the table:
|
||||
# 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty
|
||||
data_len = len(msg.data)
|
||||
if data_len == 0:
|
||||
data_type_key = 3
|
||||
elif data_len == 1:
|
||||
data_type_key = 0
|
||||
elif data_len == 2:
|
||||
data_type_key = 1
|
||||
elif data_len == 4:
|
||||
data_type_key = 2
|
||||
else:
|
||||
# Fallback: treat any other non-zero length as float32 x2
|
||||
self.logger.warn(
|
||||
f"unexpected VicCAN data length {data_len}; encoding as float32 x2 (key=1) and truncating/padding as needed"
|
||||
)
|
||||
data_type_key = 1
|
||||
|
||||
# Command is limited to 6 bits.
|
||||
command = int(msg.command_id)
|
||||
if command < 0:
|
||||
self.logger.error(f"invalid negative command_id for CAN frame: {command}")
|
||||
return
|
||||
if command > 0x3F:
|
||||
self.logger.warn(
|
||||
f"command_id 0x{command:X} exceeds 6-bit range; truncating to lower 6 bits"
|
||||
)
|
||||
command &= 0x3F
|
||||
|
||||
arbitration_id = (
|
||||
((mcu_key & 0b111) << 8) | ((data_type_key & 0b11) << 6) | (command & 0x3F)
|
||||
)
|
||||
|
||||
# Map VicCAN.data (floats) to up to 8 CAN data bytes.
|
||||
raw_bytes: list[int] = []
|
||||
for value in msg.data:
|
||||
try:
|
||||
b = int(round(value))
|
||||
except (TypeError, ValueError):
|
||||
self.logger.error(
|
||||
f"non-numeric VicCAN data value: {value}; dropping message"
|
||||
)
|
||||
return
|
||||
|
||||
if b < 0 or b > 255:
|
||||
self.logger.warn(
|
||||
f"VicCAN data value {value} out of byte range; clamping into [0, 255]"
|
||||
)
|
||||
b = max(0, min(255, b))
|
||||
|
||||
raw_bytes.append(b)
|
||||
|
||||
if len(raw_bytes) > 8:
|
||||
self.logger.warn(
|
||||
f"VicCAN data too long for single CAN frame ({len(raw_bytes)} > 8); truncating"
|
||||
)
|
||||
raw_bytes = raw_bytes[:8]
|
||||
|
||||
try:
|
||||
can_message = can.Message(
|
||||
arbitration_id=arbitration_id,
|
||||
data=raw_bytes,
|
||||
is_extended_id=False,
|
||||
)
|
||||
except Exception as e:
|
||||
self.logger.error(f"failed to construct CAN message: {e}")
|
||||
return
|
||||
|
||||
try:
|
||||
self.can_bus.send(can_message)
|
||||
self.logger.debug(
|
||||
f"sent CAN frame id=0x{can_message.arbitration_id:X}, "
|
||||
f"data={list(can_message.data)}"
|
||||
)
|
||||
except can.CanError as e:
|
||||
self.logger.error(f"CAN error while sending: {e}")
|
||||
raise DeviceClosedException("CAN bus closed unexpectedly")
|
||||
|
||||
def write_raw(self, msg: str):
|
||||
self.logger.warn(f"write_raw is not supported for CANConnector. msg: {msg}")
|
||||
|
||||
def cleanup(self):
|
||||
try:
|
||||
if self.can_bus is not None:
|
||||
self.logger.info("shutting down CAN bus")
|
||||
self.can_bus.shutdown()
|
||||
except Exception as e:
|
||||
self.logger.error(e)
|
||||
|
||||
|
||||
class MockConnector(Connector):
|
||||
def __init__(self, logger: RcutilsLogger, clock: Clock):
|
||||
super().__init__(logger, clock)
|
||||
# No hardware interface for MockConnector. Publish to `/anchor/from_vic/mock_mcu` instead.
|
||||
|
||||
def read(self) -> tuple[VicCAN | None, str | None]:
|
||||
return (None, None)
|
||||
|
||||
def write(self, msg: VicCAN):
|
||||
pass
|
||||
|
||||
def write_raw(self, msg: str):
|
||||
pass
|
||||
@@ -1,62 +0,0 @@
|
||||
from astra_msgs.msg import VicCAN
|
||||
from std_msgs.msg import Header
|
||||
from builtin_interfaces.msg import Time
|
||||
from rclpy.impl.rcutils_logger import RcutilsLogger
|
||||
|
||||
|
||||
def string_to_viccan(
|
||||
msg: str, mcu_name: str, logger: RcutilsLogger, time: Time
|
||||
) -> VicCAN | None:
|
||||
"""
|
||||
Converts the serial string VicCAN format to a ROS2 VicCAN message.
|
||||
Does not fill out the Header of the message.
|
||||
On a failure, it will log at a debug level why it failed and return None.
|
||||
"""
|
||||
|
||||
parts: list[str] = msg.strip().split(",")
|
||||
|
||||
# don't need an extra check because len of .split output is always >= 1
|
||||
if parts[0] != "can_relay_fromvic":
|
||||
logger.debug(f"got non-CAN data from {mcu_name}: {msg}")
|
||||
return None
|
||||
elif len(parts) < 3:
|
||||
logger.debug(f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}")
|
||||
return None
|
||||
elif len(parts) > 7:
|
||||
logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
|
||||
return None
|
||||
|
||||
try:
|
||||
command_id = int(parts[2])
|
||||
except ValueError:
|
||||
logger.debug(
|
||||
f"got garbage (non-integer command id) CAN data from {mcu_name}: {msg}"
|
||||
)
|
||||
return None
|
||||
|
||||
if command_id not in range(64):
|
||||
logger.debug(
|
||||
f"got garbage (wrong command id {command_id}) CAN data from {mcu_name}: {msg}"
|
||||
)
|
||||
return None
|
||||
|
||||
try:
|
||||
return VicCAN(
|
||||
header=Header(
|
||||
stamp=time,
|
||||
frame_id="from_vic",
|
||||
),
|
||||
mcu_name=parts[1],
|
||||
command_id=command_id,
|
||||
data=[float(x) for x in parts[3:]],
|
||||
)
|
||||
except ValueError:
|
||||
logger.debug(f"got garbage (non-numerical) CAN data from {mcu_name}: {msg}")
|
||||
return None
|
||||
|
||||
|
||||
def viccan_to_string(viccan: VicCAN) -> str:
|
||||
"""Converts a ROS2 VicCAN message to the serial string VicCAN format."""
|
||||
# go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max
|
||||
data = "".join([f",{round(val,7)}" for val in viccan.data])
|
||||
return f"can_relay_tovic,{viccan.mcu_name}{data}\n"
|
||||
@@ -1,111 +1,136 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, Shutdown
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
|
||||
from launch.substitutions import (
|
||||
LaunchConfiguration,
|
||||
ThisLaunchFileDir,
|
||||
PathJoinSubstitution,
|
||||
)
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
# Prevent making __pycache__ directories
|
||||
from sys import dont_write_bytecode
|
||||
|
||||
dont_write_bytecode = True
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
# Retrieve the resolved value of the launch argument 'mode'
|
||||
mode = LaunchConfiguration("mode").perform(context)
|
||||
nodes = []
|
||||
|
||||
if mode == "anchor":
|
||||
# Launch every node and pass "anchor" as the parameter
|
||||
|
||||
nodes.append(
|
||||
Node(
|
||||
package="arm_pkg",
|
||||
executable="arm", # change as needed
|
||||
name="arm",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="core_pkg",
|
||||
executable="core", # change as needed
|
||||
name="core",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="core_pkg",
|
||||
executable="ptz", # change as needed
|
||||
name="ptz",
|
||||
output="both",
|
||||
# Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
|
||||
# on_exit=Shutdown() # Uncomment if you want to shutdown on PTZ failure
|
||||
)
|
||||
)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="bio_pkg",
|
||||
executable="bio", # change as needed
|
||||
name="bio",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="anchor_pkg",
|
||||
executable="anchor", # change as needed
|
||||
name="anchor",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
elif mode in ["arm", "core", "bio", "ptz"]:
|
||||
# Only launch the node corresponding to the provided mode.
|
||||
if mode == "arm":
|
||||
nodes.append(
|
||||
Node(
|
||||
package="arm_pkg",
|
||||
executable="arm",
|
||||
name="arm",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
elif mode == "core":
|
||||
nodes.append(
|
||||
Node(
|
||||
package="core_pkg",
|
||||
executable="core",
|
||||
name="core",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
elif mode == "bio":
|
||||
nodes.append(
|
||||
Node(
|
||||
package="bio_pkg",
|
||||
executable="bio",
|
||||
name="bio",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
elif mode == "ptz":
|
||||
nodes.append(
|
||||
Node(
|
||||
package="core_pkg",
|
||||
executable="ptz",
|
||||
name="ptz",
|
||||
output="both",
|
||||
on_exit=Shutdown(), # on fail, shutdown if this was the only node to be launched
|
||||
)
|
||||
)
|
||||
else:
|
||||
# If an invalid mode is provided, print an error.
|
||||
print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
|
||||
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
connector = LaunchConfiguration("connector")
|
||||
serial_override = LaunchConfiguration("serial_override")
|
||||
can_override = LaunchConfiguration("can_override")
|
||||
use_ptz = LaunchConfiguration("use_ptz")
|
||||
|
||||
ld = LaunchDescription()
|
||||
|
||||
# arguments
|
||||
ld.add_action(
|
||||
DeclareLaunchArgument(
|
||||
"connector",
|
||||
default_value="auto",
|
||||
description="Connector parameter for anchor node (default: 'auto')",
|
||||
)
|
||||
declare_arg = DeclareLaunchArgument(
|
||||
"mode",
|
||||
default_value="anchor",
|
||||
description="Launch mode: arm, core, bio, anchor, or ptz",
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
DeclareLaunchArgument(
|
||||
"serial_override",
|
||||
default_value="",
|
||||
description="Serial port override parameter for anchor node (default: '')",
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
DeclareLaunchArgument(
|
||||
"can_override",
|
||||
default_value="auto",
|
||||
description="CAN network override parameter for anchor node (default: '')",
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
DeclareLaunchArgument(
|
||||
"use_ptz",
|
||||
default_value="true", # must be string for launch system
|
||||
description="Whether to launch PTZ node (default: true)",
|
||||
)
|
||||
)
|
||||
|
||||
# nodes
|
||||
ld.add_action(
|
||||
Node(
|
||||
package="arm_pkg",
|
||||
executable="arm",
|
||||
name="arm",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": "anchor"}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
Node(
|
||||
package="core_pkg",
|
||||
executable="core",
|
||||
name="core",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": "anchor"}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
Node(
|
||||
package="core_pkg",
|
||||
executable="ptz",
|
||||
name="ptz",
|
||||
output="both",
|
||||
condition=IfCondition(use_ptz),
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
Node(
|
||||
package="bio_pkg",
|
||||
executable="bio",
|
||||
name="bio",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": "anchor"}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
|
||||
ld.add_action(
|
||||
Node(
|
||||
package="anchor_pkg",
|
||||
executable="anchor",
|
||||
name="anchor",
|
||||
output="both",
|
||||
parameters=[
|
||||
{
|
||||
"launch_mode": "anchor",
|
||||
"connector": connector,
|
||||
"serial_override": serial_override,
|
||||
"can_override": can_override,
|
||||
}
|
||||
],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
|
||||
return ld
|
||||
return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
|
||||
|
||||
@@ -3,14 +3,13 @@
|
||||
<package format="3">
|
||||
<name>anchor_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Anchor -- ROS and CAN relay node</description>
|
||||
<maintainer email="rjm0037@uah.edu">Riley</maintainer>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
|
||||
<license>AGPL-3.0-only</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>common_interfaces</depend>
|
||||
<depend>python3-serial</depend>
|
||||
<depend>python3-can</depend>
|
||||
|
||||
<build_depend>black</build_depend>
|
||||
|
||||
|
||||
279
src/arm_pkg/arm_pkg/arm_headless.py
Executable file
279
src/arm_pkg/arm_pkg/arm_headless.py
Executable file
@@ -0,0 +1,279 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
import pygame
|
||||
|
||||
import time
|
||||
|
||||
import serial
|
||||
import sys
|
||||
import threading
|
||||
import glob
|
||||
import os
|
||||
|
||||
from std_msgs.msg import String
|
||||
from astra_msgs.msg import ControllerState
|
||||
from astra_msgs.msg import ArmManual
|
||||
from astra_msgs.msg import ArmIK
|
||||
|
||||
|
||||
os.environ["SDL_AUDIODRIVER"] = (
|
||||
"dummy" # Force pygame to use a dummy audio driver before pygame.init()
|
||||
)
|
||||
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
|
||||
|
||||
|
||||
class Headless(Node):
|
||||
def __init__(self):
|
||||
# Initalize node with name
|
||||
super().__init__("arm_headless")
|
||||
|
||||
# Depricated, kept temporarily for reference
|
||||
# self.create_timer(0.20, self.send_controls)#read and send controls
|
||||
|
||||
self.create_timer(0.1, self.send_manual)
|
||||
|
||||
# Create a publisher to publish any output the pico sends
|
||||
|
||||
# Depricated, kept temporarily for reference
|
||||
# self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10)
|
||||
self.manual_pub = self.create_publisher(ArmManual, "/arm/control/manual", 10)
|
||||
|
||||
# Create a subscriber to listen to any commands sent for the pico
|
||||
|
||||
# Depricated, kept temporarily for reference
|
||||
# self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10)
|
||||
|
||||
# self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)
|
||||
|
||||
self.laser_status = 0
|
||||
|
||||
# Initialize pygame
|
||||
pygame.init()
|
||||
|
||||
# Initialize the gamepad module
|
||||
pygame.joystick.init()
|
||||
|
||||
# Check if any gamepad is connected
|
||||
if pygame.joystick.get_count() == 0:
|
||||
print("No gamepad found.")
|
||||
pygame.quit()
|
||||
exit()
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
pygame.quit()
|
||||
exit()
|
||||
|
||||
# Initialize the first gamepad, print name to terminal
|
||||
self.gamepad = pygame.joystick.Joystick(0)
|
||||
self.gamepad.init()
|
||||
print(f"Gamepad Found: {self.gamepad.get_name()}")
|
||||
#
|
||||
#
|
||||
|
||||
def run(self):
|
||||
# This thread makes all the update processes run in the background
|
||||
thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
|
||||
thread.start()
|
||||
|
||||
try:
|
||||
while rclpy.ok():
|
||||
# Check the pico for updates
|
||||
|
||||
# self.read_feedback()
|
||||
if (
|
||||
pygame.joystick.get_count() == 0
|
||||
): # if controller disconnected, wait for it to be reconnected
|
||||
print(f"Gamepad disconnected: {self.gamepad.get_name()}")
|
||||
|
||||
while pygame.joystick.get_count() == 0:
|
||||
# self.send_controls() #depricated, kept for reference temporarily
|
||||
self.send_manual()
|
||||
# self.read_feedback()
|
||||
self.gamepad = pygame.joystick.Joystick(0)
|
||||
self.gamepad.init() # re-initialized gamepad
|
||||
print(f"Gamepad reconnected: {self.gamepad.get_name()}")
|
||||
|
||||
except KeyboardInterrupt:
|
||||
sys.exit(0)
|
||||
|
||||
def send_manual(self):
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
pygame.quit()
|
||||
exit()
|
||||
input = ArmManual()
|
||||
|
||||
# Triggers for gripper control
|
||||
if self.gamepad.get_axis(2) > 0: # left trigger
|
||||
input.gripper = -1
|
||||
elif self.gamepad.get_axis(5) > 0: # right trigger
|
||||
input.gripper = 1
|
||||
|
||||
# Toggle Laser
|
||||
if self.gamepad.get_button(7): # Start
|
||||
self.laser_status = 1
|
||||
elif self.gamepad.get_button(6): # Back
|
||||
self.laser_status = 0
|
||||
input.laser = self.laser_status
|
||||
|
||||
if self.gamepad.get_button(5): # right bumper, control effector
|
||||
|
||||
# Left stick X-axis for effector yaw
|
||||
if self.gamepad.get_axis(0) > 0:
|
||||
input.effector_yaw = 1
|
||||
elif self.gamepad.get_axis(0) < 0:
|
||||
input.effector_yaw = -1
|
||||
|
||||
# Right stick X-axis for effector roll
|
||||
if self.gamepad.get_axis(3) > 0:
|
||||
input.effector_roll = 1
|
||||
elif self.gamepad.get_axis(3) < 0:
|
||||
input.effector_roll = -1
|
||||
|
||||
else: # Control arm axis
|
||||
dpad_input = self.gamepad.get_hat(0)
|
||||
input.axis0 = 0
|
||||
if dpad_input[0] == 1:
|
||||
input.axis0 = 1
|
||||
elif dpad_input[0] == -1:
|
||||
input.axis0 = -1
|
||||
|
||||
if self.gamepad.get_axis(0) > 0.15 or self.gamepad.get_axis(0) < -0.15:
|
||||
input.axis1 = round(self.gamepad.get_axis(0))
|
||||
|
||||
if self.gamepad.get_axis(1) > 0.15 or self.gamepad.get_axis(1) < -0.15:
|
||||
input.axis2 = -1 * round(self.gamepad.get_axis(1))
|
||||
|
||||
if self.gamepad.get_axis(4) > 0.15 or self.gamepad.get_axis(4) < -0.15:
|
||||
input.axis3 = -1 * round(self.gamepad.get_axis(4))
|
||||
|
||||
# input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis
|
||||
# input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis
|
||||
# input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis
|
||||
|
||||
# Button Mappings
|
||||
# axis2 -> LT
|
||||
# axis5 -> RT
|
||||
# Buttons0 -> A
|
||||
# Buttons1 -> B
|
||||
# Buttons2 -> X
|
||||
# Buttons3 -> Y
|
||||
# Buttons4 -> LB
|
||||
# Buttons5 -> RB
|
||||
# Buttons6 -> Back
|
||||
# Buttons7 -> Start
|
||||
|
||||
input.linear_actuator = 0
|
||||
|
||||
if pygame.joystick.get_count() != 0:
|
||||
|
||||
self.get_logger().info(
|
||||
f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n"
|
||||
)
|
||||
self.manual_pub.publish(input)
|
||||
else:
|
||||
pass
|
||||
|
||||
pass
|
||||
|
||||
# Depricated, kept temporarily for reference
|
||||
# def send_controls(self):
|
||||
|
||||
# for event in pygame.event.get():
|
||||
# if event.type == pygame.QUIT:
|
||||
# pygame.quit()
|
||||
# exit()
|
||||
# input = ControllerState()
|
||||
|
||||
# input.lt = self.gamepad.get_axis(2)#left trigger
|
||||
# input.rt = self.gamepad.get_axis(5)#right trigger
|
||||
|
||||
# #input.lb = self.gamepad.get_button(9)#Value must be converted to bool
|
||||
# if(self.gamepad.get_button(4)):#left bumper
|
||||
# input.lb = True
|
||||
# else:
|
||||
# input.lb = False
|
||||
|
||||
# #input.rb = self.gamepad.get_button(10)#Value must be converted to bool
|
||||
# if(self.gamepad.get_button(5)):#right bumper
|
||||
# input.rb = True
|
||||
# else:
|
||||
# input.rb = False
|
||||
|
||||
# #input.plus = self.gamepad.get_button(6)#plus button
|
||||
# if(self.gamepad.get_button(7)):#plus button
|
||||
# input.plus = True
|
||||
# else:
|
||||
# input.plus = False
|
||||
|
||||
# #input.minus = self.gamepad.get_button(4)#minus button
|
||||
# if(self.gamepad.get_button(6)):#minus button
|
||||
# input.minus = True
|
||||
# else:
|
||||
# input.minus = False
|
||||
|
||||
# input.ls_x = round(self.gamepad.get_axis(0),2)#left x-axis
|
||||
# input.ls_y = round(self.gamepad.get_axis(1),2)#left y-axis
|
||||
# input.rs_x = round(self.gamepad.get_axis(3),2)#right x-axis
|
||||
# input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis
|
||||
|
||||
# #input.a = self.gamepad.get_button(1)#A button
|
||||
# if(self.gamepad.get_button(0)):#A button
|
||||
# input.a = True
|
||||
# else:
|
||||
# input.a = False
|
||||
# #input.b = self.gamepad.get_button(0)#B button
|
||||
# if(self.gamepad.get_button(1)):#B button
|
||||
# input.b = True
|
||||
# else:
|
||||
# input.b = False
|
||||
# #input.x = self.gamepad.get_button(3)#X button
|
||||
# if(self.gamepad.get_button(2)):#X button
|
||||
# input.x = True
|
||||
# else:
|
||||
# input.x = False
|
||||
# #input.y = self.gamepad.get_button(2)#Y button
|
||||
# if(self.gamepad.get_button(3)):#Y button
|
||||
# input.y = True
|
||||
# else:
|
||||
# input.y = False
|
||||
|
||||
# dpad_input = self.gamepad.get_hat(0)#D-pad input
|
||||
|
||||
# #not using up/down on DPad
|
||||
# input.d_up = False
|
||||
# input.d_down = False
|
||||
|
||||
# if(dpad_input[0] == 1):#D-pad right
|
||||
# input.d_right = True
|
||||
# else:
|
||||
# input.d_right = False
|
||||
# if(dpad_input[0] == -1):#D-pad left
|
||||
# input.d_left = True
|
||||
# else:
|
||||
# input.d_left = False
|
||||
|
||||
# if pygame.joystick.get_count() != 0:
|
||||
|
||||
# self.get_logger().info(f"[Ctrl] Updated Controller State\n")
|
||||
|
||||
# self.publisher.publish(input)
|
||||
# else:
|
||||
# pass
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
node = Headless()
|
||||
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
|
||||
# tb_bs = BaseStation()
|
||||
# node.run()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,221 +1,181 @@
|
||||
import sys
|
||||
import threading
|
||||
import signal
|
||||
import math
|
||||
from warnings import deprecated
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy import qos
|
||||
|
||||
from std_msgs.msg import String, Header
|
||||
import serial
|
||||
import sys
|
||||
import threading
|
||||
import glob
|
||||
import time
|
||||
import atexit
|
||||
import signal
|
||||
from std_msgs.msg import String
|
||||
from astra_msgs.msg import ArmManual
|
||||
from astra_msgs.msg import SocketFeedback
|
||||
from astra_msgs.msg import DigitFeedback
|
||||
from sensor_msgs.msg import JointState
|
||||
from control_msgs.msg import JointJog
|
||||
from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual # TODO: Old topics
|
||||
from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState
|
||||
import math
|
||||
|
||||
control_qos = qos.QoSProfile(
|
||||
history=qos.QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=2,
|
||||
reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, # Best Effort subscribers are still compatible with Reliable publishers
|
||||
durability=qos.QoSDurabilityPolicy.VOLATILE,
|
||||
# deadline=Duration(seconds=1),
|
||||
# lifespan=Duration(nanoseconds=500_000_000), # 500ms
|
||||
# liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
|
||||
# liveliness_lease_duration=Duration(seconds=5),
|
||||
)
|
||||
# control_qos = qos.QoSProfile(
|
||||
# history=qos.QoSHistoryPolicy.KEEP_LAST,
|
||||
# depth=1,
|
||||
# reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
|
||||
# durability=qos.QoSDurabilityPolicy.VOLATILE,
|
||||
# deadline=1000,
|
||||
# lifespan=500,
|
||||
# liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
|
||||
# liveliness_lease_duration=5000
|
||||
# )
|
||||
|
||||
serial_pub = None
|
||||
thread = None
|
||||
|
||||
|
||||
class ArmNode(Node):
|
||||
"""Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics."""
|
||||
|
||||
# Every non-fixed joint defined in Arm's URDF
|
||||
# Used for JointState and JointJog messsages
|
||||
all_joint_names = [
|
||||
"axis_0_joint",
|
||||
"axis_1_joint",
|
||||
"axis_2_joint",
|
||||
"axis_3_joint",
|
||||
"wrist_yaw_joint",
|
||||
"wrist_roll_joint",
|
||||
"ef_gripper_left_joint",
|
||||
]
|
||||
|
||||
# Used to verify the length of an incoming VicCAN feedback message
|
||||
# Key is VicCAN command_id, value is expected length of data list
|
||||
viccan_socket_msg_len_dict = {
|
||||
53: 4,
|
||||
54: 4,
|
||||
55: 4,
|
||||
58: 4,
|
||||
59: 4,
|
||||
}
|
||||
|
||||
viccan_digit_msg_len_dict = {
|
||||
54: 4,
|
||||
55: 2,
|
||||
59: 2,
|
||||
}
|
||||
|
||||
class SerialRelay(Node):
|
||||
def __init__(self):
|
||||
# Initialize node
|
||||
super().__init__("arm_node")
|
||||
|
||||
self.get_logger().info(f"arm launch_mode is: anchor") # Hey I like the output
|
||||
# Get launch mode parameter
|
||||
self.declare_parameter("launch_mode", "arm")
|
||||
self.launch_mode = self.get_parameter("launch_mode").value
|
||||
self.get_logger().info(f"arm launch_mode is: {self.launch_mode}")
|
||||
|
||||
##################################################
|
||||
# Parameters
|
||||
# Create publishers
|
||||
self.debug_pub = self.create_publisher(String, "/arm/feedback/debug", 10)
|
||||
self.socket_pub = self.create_publisher(
|
||||
SocketFeedback, "/arm/feedback/socket", 10
|
||||
)
|
||||
self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
|
||||
self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
|
||||
|
||||
self.declare_parameter("use_old_topics", True)
|
||||
self.use_old_topics = (
|
||||
self.get_parameter("use_old_topics").get_parameter_value().bool_value
|
||||
# Create subscribers
|
||||
self.man_sub = self.create_subscription(
|
||||
ArmManual, "/arm/control/manual", self.send_manual, 2
|
||||
)
|
||||
|
||||
##################################################
|
||||
# Old topics
|
||||
# New messages
|
||||
self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
|
||||
self.joint_state = JointState()
|
||||
self.joint_state.name = [
|
||||
"Axis_0_Joint",
|
||||
"Axis_1_Joint",
|
||||
"Axis_2_Joint",
|
||||
"Axis_3_Joint",
|
||||
"Wrist_Differential_Joint",
|
||||
"Wrist-EF_Roll_Joint",
|
||||
"Gripper_Slider_Left",
|
||||
]
|
||||
self.joint_state.position = [0.0] * len(
|
||||
self.joint_state.name
|
||||
) # Initialize with zeros
|
||||
|
||||
if self.use_old_topics:
|
||||
# Anchor topics
|
||||
self.joint_command_sub = self.create_subscription(
|
||||
JointState, "/joint_commands", self.joint_command_callback, 10
|
||||
)
|
||||
|
||||
# Topics used in anchor mode
|
||||
if self.launch_mode == "anchor":
|
||||
self.anchor_sub = self.create_subscription(
|
||||
String, "/anchor/arm/feedback", self.anchor_feedback, 10
|
||||
)
|
||||
self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
|
||||
|
||||
# Create publishers
|
||||
self.socket_pub = self.create_publisher(
|
||||
SocketFeedback, "/arm/feedback/socket", 10
|
||||
)
|
||||
self.arm_feedback = SocketFeedback()
|
||||
self.digit_pub = self.create_publisher(
|
||||
DigitFeedback, "/arm/feedback/digit", 10
|
||||
)
|
||||
self.digit_feedback = DigitFeedback()
|
||||
self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
|
||||
self.arm_feedback = SocketFeedback()
|
||||
self.digit_feedback = DigitFeedback()
|
||||
|
||||
# Create subscribers
|
||||
self.man_sub = self.create_subscription(
|
||||
ArmManual, "/arm/control/manual", self.send_manual, 10
|
||||
)
|
||||
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode
|
||||
if self.launch_mode == "arm":
|
||||
# Loop through all serial devices on the computer to check for the MCU
|
||||
self.port = None
|
||||
ports = SerialRelay.list_serial_ports()
|
||||
for _ in range(4):
|
||||
for port in ports:
|
||||
try:
|
||||
# connect and send a ping command
|
||||
ser = serial.Serial(port, 115200, timeout=1)
|
||||
# print(f"Checking port {port}...")
|
||||
ser.write(b"ping\n")
|
||||
response = ser.read_until("\n") # type: ignore
|
||||
|
||||
###################################################
|
||||
# New topics
|
||||
# if pong is in response, then we are talking with the MCU
|
||||
if b"pong" in response:
|
||||
self.port = port
|
||||
self.get_logger().info(f"Found MCU at {self.port}!")
|
||||
break
|
||||
except:
|
||||
pass
|
||||
if self.port is not None:
|
||||
break
|
||||
|
||||
# Anchor topics
|
||||
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)
|
||||
|
||||
# from_vic
|
||||
self.anchor_fromvic_sub_ = self.create_subscription(
|
||||
VicCAN, "/anchor/from_vic/arm", self.relay_fromvic, 20
|
||||
)
|
||||
# to_vic
|
||||
self.anchor_tovic_pub_ = self.create_publisher(
|
||||
VicCAN, "/anchor/to_vic/relay", 20
|
||||
)
|
||||
self.ser = serial.Serial(self.port, 115200)
|
||||
atexit.register(self.cleanup)
|
||||
|
||||
# Control
|
||||
def run(self):
|
||||
global thread
|
||||
thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
|
||||
thread.start()
|
||||
|
||||
# Manual: /arm/manual/joint_jog is published by Basestation or Headless
|
||||
self.man_jointjog_sub_ = self.create_subscription(
|
||||
JointJog,
|
||||
"/arm/manual/joint_jog",
|
||||
self.jointjog_callback,
|
||||
qos_profile=control_qos,
|
||||
)
|
||||
# IK: /joint_commands is published by JointTrajectoryController via topic_based_control
|
||||
self.joint_command_sub_ = self.create_subscription(
|
||||
JointState,
|
||||
"/joint_commands",
|
||||
self.joint_command_callback,
|
||||
qos_profile=control_qos,
|
||||
)
|
||||
# if in arm mode, will need to read from the MCU
|
||||
|
||||
# Feedback
|
||||
try:
|
||||
while rclpy.ok():
|
||||
if self.launch_mode == "arm":
|
||||
if self.ser.in_waiting:
|
||||
self.read_mcu()
|
||||
else:
|
||||
time.sleep(0.1)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
self.cleanup()
|
||||
|
||||
# Combined Socket and Digit feedback
|
||||
self.arm_feedback_pub_ = self.create_publisher(
|
||||
ArmFeedback,
|
||||
"/arm/feedback",
|
||||
qos_profile=qos.qos_profile_sensor_data,
|
||||
)
|
||||
# IK arm pose: /joint_states is published from here to topic_based_control
|
||||
self.joint_state_pub_ = self.create_publisher(
|
||||
JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
|
||||
)
|
||||
|
||||
###################################################
|
||||
# Saved state
|
||||
|
||||
# Combined Socket and Digit feedback
|
||||
self.arm_feedback_new = ArmFeedback()
|
||||
|
||||
# IK Arm pose
|
||||
self.saved_joint_state = JointState()
|
||||
self.saved_joint_state.name = self.all_joint_names
|
||||
# ... initialize with zeros
|
||||
self.saved_joint_state.position = [0.0] * len(self.saved_joint_state.name)
|
||||
self.saved_joint_state.velocity = [0.0] * len(self.saved_joint_state.name)
|
||||
|
||||
def jointjog_callback(self, msg: JointJog):
|
||||
if len(msg.joint_names) != len(msg.velocities):
|
||||
self.get_logger().debug("Ignoring malformed /arm/manual/joint_jog message.")
|
||||
return
|
||||
|
||||
# Grab velocities from message
|
||||
velocities = [
|
||||
(
|
||||
msg.velocities[msg.joint_names.index(joint_name)] # type: ignore
|
||||
if joint_name in msg.joint_names
|
||||
else 0.0
|
||||
)
|
||||
for joint_name in self.all_joint_names
|
||||
]
|
||||
# Deadzone
|
||||
velocities = [vel if abs(vel) > 0.05 else 0.0 for vel in velocities]
|
||||
|
||||
self.send_velocities(velocities, msg.header)
|
||||
|
||||
# TODO: use msg.duration
|
||||
# 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 joint_command_callback(self, msg: JointState):
|
||||
if len(msg.position) < 7 and len(msg.velocity) < 7:
|
||||
self.get_logger().debug("Ignoring malformed /joint_command message.")
|
||||
return # command needs either position or velocity for all 7 joints
|
||||
# Embedded takes deg*10, ROS2 uses Radians
|
||||
positions = [math.degrees(pos) * 10 for pos in msg.position]
|
||||
# Axis 2 & 3 URDF direction is inverted
|
||||
positions[2] = -positions[2]
|
||||
positions[3] = -positions[3]
|
||||
|
||||
# Grab velocities from message
|
||||
velocities = [
|
||||
(
|
||||
msg.velocity[msg.name.index(joint_name)] # type: ignore
|
||||
if joint_name in msg.name
|
||||
else 0.0
|
||||
)
|
||||
for joint_name in self.all_joint_names
|
||||
]
|
||||
# Set target angles for each arm axis for embedded IK PID to handle
|
||||
command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
|
||||
# Wrist yaw and roll
|
||||
command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
|
||||
# Gripper IK does not have adequate hardware yet
|
||||
self.send_cmd(command)
|
||||
|
||||
self.send_velocities(velocities, msg.header)
|
||||
|
||||
def send_velocities(self, velocities: list[float], header: Header):
|
||||
# ROS2's rad/s to VicCAN's deg/s*10; don't convert gripper's m/s
|
||||
velocities = [
|
||||
math.degrees(vel) * 10 if i < 6 else vel for i, vel in enumerate(velocities)
|
||||
]
|
||||
|
||||
# Send Axis 0-3
|
||||
self.anchor_tovic_pub_.publish(
|
||||
VicCAN(mcu_name="arm", command_id=43, data=velocities[0:3], header=header)
|
||||
)
|
||||
# Send Wrist yaw and roll
|
||||
# TODO: Verify embedded
|
||||
self.anchor_tovic_pub_.publish(
|
||||
VicCAN(mcu_name="digit", command_id=43, data=velocities[4:5], header=header)
|
||||
)
|
||||
# Send End Effector Gripper
|
||||
# TODO: Verify m/s received correctly by embedded
|
||||
self.anchor_tovic_pub_.publish(
|
||||
VicCAN(mcu_name="digit", command_id=26, data=[velocities[6]], header=header)
|
||||
)
|
||||
|
||||
@deprecated("Uses an old message type. Will be removed at some point.")
|
||||
def send_manual(self, msg: ArmManual):
|
||||
axis0 = msg.axis0
|
||||
axis1 = -1 * msg.axis1
|
||||
@@ -240,17 +200,24 @@ class ArmNode(Node):
|
||||
|
||||
return
|
||||
|
||||
@deprecated("Uses an old message type. Will be removed at some point.")
|
||||
def send_cmd(self, msg: str):
|
||||
output = String(data=msg)
|
||||
self.anchor_pub.publish(output)
|
||||
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 == "arm": # if in standalone mode, send to MCU directly
|
||||
self.get_logger().info(f"[Arm to MCU] {msg}")
|
||||
self.ser.write(bytes(msg, "utf8"))
|
||||
|
||||
@deprecated("Uses an old message type. Will be removed at some point.")
|
||||
def anchor_feedback(self, msg: String):
|
||||
output = msg.data
|
||||
if output.startswith("can_relay_fromvic,arm,55"):
|
||||
# pass
|
||||
self.updateAngleFeedback(output)
|
||||
elif output.startswith("can_relay_fromvic,arm,54"):
|
||||
# pass
|
||||
self.updateBusVoltage(output)
|
||||
elif output.startswith("can_relay_fromvic,arm,53"):
|
||||
self.updateMotorFeedback(output)
|
||||
@@ -268,133 +235,19 @@ class ArmNode(Node):
|
||||
if len(parts) >= 4:
|
||||
self.digit_feedback.wrist_angle = float(parts[3])
|
||||
# self.digit_feedback.wrist_roll = float(parts[4])
|
||||
self.joint_state.position[4] = math.radians(
|
||||
float(parts[4])
|
||||
) # Wrist roll
|
||||
self.joint_state.position[5] = math.radians(
|
||||
float(parts[3])
|
||||
) # Wrist yaw
|
||||
else:
|
||||
return
|
||||
|
||||
def relay_fromvic(self, msg: VicCAN):
|
||||
# Code for socket and digit are broken out for cleaner code
|
||||
if msg.mcu_name == "arm":
|
||||
self.process_fromvic_arm(msg)
|
||||
elif msg.mcu_name == "digit":
|
||||
self.process_fromvic_digit(msg)
|
||||
|
||||
def process_fromvic_arm(self, msg: VicCAN):
|
||||
assert msg.mcu_name == "arm"
|
||||
|
||||
# Check message len to prevent crashing on bad data
|
||||
if msg.command_id in self.viccan_socket_msg_len_dict:
|
||||
expected_len = self.viccan_socket_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
|
||||
|
||||
match msg.command_id:
|
||||
case 53: # REV SPARK MAX feedback
|
||||
motorId = round(msg.data[0])
|
||||
motor: RevMotorState | None = None
|
||||
match motorId:
|
||||
case 1:
|
||||
motor = self.arm_feedback_new.axis1_motor
|
||||
case 2:
|
||||
motor = self.arm_feedback_new.axis2_motor
|
||||
case 3:
|
||||
motor = self.arm_feedback_new.axis3_motor
|
||||
case 4:
|
||||
motor = self.arm_feedback_new.axis0_motor
|
||||
|
||||
if motor:
|
||||
motor.temperature = float(msg.data[1]) / 10.0
|
||||
motor.voltage = float(msg.data[2]) / 10.0
|
||||
motor.current = float(msg.data[3]) / 10.0
|
||||
motor.header.stamp = msg.header.stamp
|
||||
|
||||
self.arm_feedback_pub_.publish(self.arm_feedback_new)
|
||||
case 54: # Board voltages
|
||||
self.arm_feedback_new.socket_voltage.vbatt = float(msg.data[0]) / 100.0
|
||||
self.arm_feedback_new.socket_voltage.v12 = float(msg.data[1]) / 100.0
|
||||
self.arm_feedback_new.socket_voltage.v5 = float(msg.data[2]) / 100.0
|
||||
self.arm_feedback_new.socket_voltage.v3 = float(msg.data[3]) / 100.0
|
||||
self.arm_feedback_new.socket_voltage.header.stamp = msg.header.stamp
|
||||
case 55: # Arm joint positions
|
||||
angles = [angle / 10.0 for angle in msg.data] # VicCAN sends deg*10
|
||||
# Joint state publisher for URDF visualization
|
||||
self.saved_joint_state.position[0] = math.radians(angles[0]) # Axis 0
|
||||
self.saved_joint_state.position[1] = math.radians(angles[1]) # Axis 1
|
||||
self.saved_joint_state.position[2] = math.radians(angles[2]) # Axis 2
|
||||
self.saved_joint_state.position[3] = math.radians(angles[3]) # Axis 3
|
||||
# Wrist is handled by digit feedback
|
||||
self.saved_joint_state.header.stamp = msg.header.stamp
|
||||
self.joint_state_pub_.publish(self.saved_joint_state)
|
||||
case 58: # REV SPARK MAX position and velocity feedback
|
||||
motorId = round(msg.data[0])
|
||||
motor: RevMotorState | None = None
|
||||
match motorId:
|
||||
case 1:
|
||||
motor = self.arm_feedback_new.axis1_motor
|
||||
case 2:
|
||||
motor = self.arm_feedback_new.axis2_motor
|
||||
case 3:
|
||||
motor = self.arm_feedback_new.axis3_motor
|
||||
case 4:
|
||||
motor = self.arm_feedback_new.axis0_motor
|
||||
|
||||
if motor:
|
||||
motor.position = float(msg.data[1])
|
||||
motor.velocity = float(msg.data[2])
|
||||
motor.header.stamp = msg.header.stamp
|
||||
|
||||
self.arm_feedback_pub_.publish(self.arm_feedback_new)
|
||||
case 59: # Arm joint velocities
|
||||
velocities = [vel / 100.0 for vel in msg.data] # VicCAN sends deg/s*100
|
||||
self.saved_joint_state.velocity[0] = math.radians(
|
||||
velocities[0]
|
||||
) # Axis 0
|
||||
self.saved_joint_state.velocity[1] = math.radians(
|
||||
velocities[1]
|
||||
) # Axis 1
|
||||
self.saved_joint_state.velocity[2] = math.radians(
|
||||
velocities[2]
|
||||
) # Axis 2
|
||||
self.saved_joint_state.velocity[3] = math.radians(
|
||||
velocities[3]
|
||||
) # Axis 3
|
||||
# Wrist is handled by digit feedback
|
||||
self.saved_joint_state.header.stamp = msg.header.stamp
|
||||
self.joint_state_pub_.publish(self.saved_joint_state)
|
||||
|
||||
def process_fromvic_digit(self, msg: VicCAN):
|
||||
assert msg.mcu_name == "digit"
|
||||
|
||||
# Check message len to prevent crashing on bad data
|
||||
if msg.command_id in self.viccan_digit_msg_len_dict:
|
||||
expected_len = self.viccan_digit_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
|
||||
|
||||
match msg.command_id:
|
||||
case 54: # Board voltages
|
||||
self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0
|
||||
self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0
|
||||
self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0
|
||||
case 55: # Arm joint positions
|
||||
self.saved_joint_state.position[4] = math.radians(
|
||||
msg.data[0]
|
||||
) # Wrist roll
|
||||
self.saved_joint_state.position[5] = math.radians(
|
||||
msg.data[1]
|
||||
) # Wrist yaw
|
||||
|
||||
@deprecated("Uses an old message type. Will be removed at some point.")
|
||||
def publish_feedback(self):
|
||||
self.socket_pub.publish(self.arm_feedback)
|
||||
self.digit_pub.publish(self.digit_feedback)
|
||||
|
||||
@deprecated("Uses an old message type. Will be removed at some point.")
|
||||
def updateAngleFeedback(self, msg: str):
|
||||
# Angle feedbacks,
|
||||
# split the msg.data by commas
|
||||
@@ -410,10 +263,19 @@ class ArmNode(Node):
|
||||
self.arm_feedback.axis1_angle = angles[1]
|
||||
self.arm_feedback.axis2_angle = angles[2]
|
||||
self.arm_feedback.axis3_angle = angles[3]
|
||||
|
||||
# Joint state publisher for URDF visualization
|
||||
self.joint_state.position[0] = math.radians(angles[0]) # Axis 0
|
||||
self.joint_state.position[1] = math.radians(angles[1]) # Axis 1
|
||||
self.joint_state.position[2] = math.radians(-angles[2]) # Axis 2 (inverted)
|
||||
self.joint_state.position[3] = math.radians(-angles[3]) # Axis 3 (inverted)
|
||||
# Wrist is handled by digit feedback
|
||||
self.joint_state.header.stamp = self.get_clock().now().to_msg()
|
||||
self.joint_state_pub.publish(self.joint_state)
|
||||
|
||||
else:
|
||||
self.get_logger().info("Invalid angle feedback input format")
|
||||
|
||||
@deprecated("Uses an old message type. Will be removed at some point.")
|
||||
def updateBusVoltage(self, msg: str):
|
||||
# Bus Voltage feedbacks
|
||||
parts = msg.split(",")
|
||||
@@ -428,7 +290,6 @@ class ArmNode(Node):
|
||||
else:
|
||||
self.get_logger().info("Invalid voltage feedback input format")
|
||||
|
||||
@deprecated("Uses an old message type. Will be removed at some point.")
|
||||
def updateMotorFeedback(self, msg: str):
|
||||
parts = str(msg.strip()).split(",")
|
||||
motorId = round(float(parts[3]))
|
||||
@@ -452,28 +313,38 @@ class ArmNode(Node):
|
||||
self.arm_feedback.axis0_voltage = voltage
|
||||
self.arm_feedback.axis0_current = current
|
||||
|
||||
@staticmethod
|
||||
def list_serial_ports():
|
||||
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
|
||||
# return glob.glob("/dev/tty[A-Za-z]*")
|
||||
|
||||
def exit_handler(signum, frame):
|
||||
print("Caught SIGTERM. Exiting...")
|
||||
rclpy.try_shutdown()
|
||||
sys.exit(0)
|
||||
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):
|
||||
rclpy.init(args=args)
|
||||
sys.excepthook = myexcepthook
|
||||
|
||||
# Catch termination signals and exit cleanly
|
||||
signal.signal(signal.SIGTERM, exit_handler)
|
||||
|
||||
arm_node = ArmNode()
|
||||
|
||||
try:
|
||||
rclpy.spin(arm_node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
rclpy.try_shutdown()
|
||||
global serial_pub
|
||||
serial_pub = SerialRelay()
|
||||
serial_pub.run()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly
|
||||
signal.signal(
|
||||
signal.SIGTERM, lambda signum, frame: sys.exit(0)
|
||||
) # Catch termination signals and exit cleanly
|
||||
main()
|
||||
|
||||
@@ -1,22 +1,27 @@
|
||||
from setuptools import setup
|
||||
from setuptools import find_packages, setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = "arm_pkg"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="1.0.0",
|
||||
packages=[package_name],
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="David",
|
||||
maintainer_email="ds0196@uah.edu",
|
||||
description="Relays topics related to the arm between VicCAN (through Anchor) and basestation.",
|
||||
license="AGPL-3.0-only",
|
||||
maintainer="tristan",
|
||||
maintainer_email="tristanmcginnis26@gmail.com",
|
||||
description="TODO: Package description",
|
||||
license="All Rights Reserved",
|
||||
entry_points={
|
||||
"console_scripts": ["arm = arm_pkg.arm_node:main"],
|
||||
"console_scripts": [
|
||||
"arm = arm_pkg.arm_node:main",
|
||||
"headless = arm_pkg.arm_headless:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
Submodule src/astra_descriptions updated: c36bd8233d...29a3eea9c5
Submodule src/astra_msgs deleted from 2264a2cb67
@@ -1,241 +1,251 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import signal
|
||||
import sys
|
||||
import threading
|
||||
import os
|
||||
import glob
|
||||
import time
|
||||
import atexit
|
||||
import signal
|
||||
from std_msgs.msg import String
|
||||
from astra_msgs.msg import BioControl
|
||||
from astra_msgs.msg import BioFeedback
|
||||
|
||||
import rclpy
|
||||
from astra_msgs.action import BioVacuum
|
||||
from astra_msgs.msg import CitadelControl, FaerieControl, VicCAN
|
||||
from astra_msgs.srv import BioTestTube, LibsSystem
|
||||
from rclpy.action import ActionServer
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Header, String
|
||||
|
||||
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")
|
||||
|
||||
# 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
|
||||
# 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
|
||||
)
|
||||
|
||||
# Create a publisher for telemetry
|
||||
self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
|
||||
self.anchor_sub = self.create_subscription(
|
||||
String, "/anchor/bio/feedback", self.anchor_feedback, 10
|
||||
)
|
||||
self.anchor_pub = self.create_publisher(String, "/anchor/relay", 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)
|
||||
# Messages
|
||||
self.citadel_sub = self.create_subscription(
|
||||
CitadelControl,
|
||||
"/bio/control/citadel",
|
||||
self.citadel_callback,
|
||||
10,
|
||||
)
|
||||
|
||||
self.bio_feedback = BioFeedback()
|
||||
self.faerie_sub = self.create_subscription(
|
||||
FaerieControl,
|
||||
"/bio/control/faerie",
|
||||
self.faerie_callback,
|
||||
10,
|
||||
)
|
||||
|
||||
# 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")
|
||||
# Services
|
||||
self.test_tube_service = self.create_service(
|
||||
BioTestTube, "/bio/control/test_tube", self.test_tube_callback
|
||||
)
|
||||
self.libs_service = self.create_service(
|
||||
LibsSystem, "bio/control/libs_system", self.libs_callback
|
||||
)
|
||||
|
||||
# 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)
|
||||
# Actions
|
||||
self._action_server = ActionServer(
|
||||
self, BioVacuum, "/bio/actions/vacuum", self.execute_vacuum
|
||||
)
|
||||
|
||||
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():
|
||||
if self.launch_mode == "bio":
|
||||
if self.ser.in_waiting:
|
||||
self.read_mcu()
|
||||
else:
|
||||
time.sleep(0.1)
|
||||
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):
|
||||
# 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):
|
||||
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"))
|
||||
# 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
|
||||
|
||||
def anchor_feedback(self, msg: String):
|
||||
output = msg.data
|
||||
parts = str(output.strip()).split(",")
|
||||
# self.get_logger().info(f"[Bio Anchor] {msg.data}")
|
||||
self.get_logger().info(f"[Bio Anchor] {msg.data}")
|
||||
# no data planned to be received from citadel, not sure about lance
|
||||
|
||||
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 citadel_callback(self, msg: CitadelControl):
|
||||
distributor_arr = msg.distributor_id
|
||||
# Distributor Control
|
||||
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)
|
||||
# Move Scythe
|
||||
vic_cmd = VicCAN(
|
||||
header=Header(stamp=self.get_clock().now().to_msg()),
|
||||
mcu_name="digit",
|
||||
command_id=42,
|
||||
data=[float(msg.move_scythe)],
|
||||
)
|
||||
self.anchor_tovic_pub_.publish(vic_cmd)
|
||||
|
||||
def publish_feedback(self):
|
||||
self.feedback_pub.publish(self.bio_feedback)
|
||||
def faerie_callback(self, msg: FaerieControl):
|
||||
# Move Faerie
|
||||
vic_cmd = VicCAN(
|
||||
header=Header(stamp=self.get_clock().now().to_msg()),
|
||||
mcu_name="digit",
|
||||
command_id=42,
|
||||
data=[float(msg.move_faerie)],
|
||||
)
|
||||
self.anchor_tovic_pub_.publish(vic_cmd)
|
||||
# Drill Speed
|
||||
vic_cmd = VicCAN(
|
||||
header=Header(stamp=self.get_clock().now().to_msg()),
|
||||
mcu_name="digit",
|
||||
command_id=19,
|
||||
data=[
|
||||
float(msg.drill_speed * 100)
|
||||
], # change on embedded so we can go (-1,1)
|
||||
)
|
||||
self.anchor_tovic_pub_.publish(vic_cmd)
|
||||
# Vanity Laser Control
|
||||
vic_cmd = VicCAN(
|
||||
header=Header(stamp=self.get_clock().now().to_msg()),
|
||||
mcu_name="digit",
|
||||
command_id=28,
|
||||
data=[float(msg.vanity_laser)],
|
||||
)
|
||||
self.anchor_tovic_pub_.publish(vic_cmd)
|
||||
|
||||
@staticmethod
|
||||
def list_serial_ports():
|
||||
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
|
||||
# return glob.glob("/dev/tty[A-Za-z]*")
|
||||
def test_tube_callback(self, request, response):
|
||||
vic_cmd = VicCAN(
|
||||
header=Header(stamp=self.get_clock().now().to_msg()),
|
||||
mcu_name="citadel",
|
||||
command_id=40,
|
||||
data=[
|
||||
float(int(request.tube_id)),
|
||||
float(request.milliliters),
|
||||
],
|
||||
)
|
||||
self.anchor_tovic_pub_.publish(vic_cmd)
|
||||
return response
|
||||
|
||||
def cleanup(self):
|
||||
print("Cleaning up...")
|
||||
try:
|
||||
if self.ser.is_open:
|
||||
self.ser.close()
|
||||
except Exception as e:
|
||||
exit(0)
|
||||
def libs_callback(self, request, response):
|
||||
print("todo")
|
||||
|
||||
def execute_vacuum(self, goal_handle):
|
||||
valve_id = int(goal_handle.request.valve_id)
|
||||
duty = int(goal_handle.request.fan_duty_cycle_percent)
|
||||
total = goal_handle.request.fan_time_ms
|
||||
|
||||
# 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)],
|
||||
)
|
||||
)
|
||||
|
||||
feedback = BioVacuum.Feedback()
|
||||
start = time.time()
|
||||
|
||||
while True:
|
||||
# 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)],
|
||||
)
|
||||
)
|
||||
|
||||
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 myexcepthook(type, value, tb):
|
||||
print("Uncaught exception:", type, value)
|
||||
if serial_pub:
|
||||
serial_pub.cleanup()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
|
||||
@@ -31,10 +31,10 @@ CORE_WHEEL_RADIUS = 0.171 # meters
|
||||
CORE_GEAR_RATIO = 100.0 # Clucky: 100:1, Testbed: 64:1
|
||||
|
||||
control_qos = qos.QoSProfile(
|
||||
history=qos.QoSHistoryPolicy.KEEP_LAST,
|
||||
# history=qos.QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=2,
|
||||
reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, # Best Effort subscribers are still compatible with Reliable publishers
|
||||
durability=qos.QoSDurabilityPolicy.VOLATILE,
|
||||
# reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
|
||||
# durability=qos.QoSDurabilityPolicy.VOLATILE,
|
||||
# deadline=Duration(seconds=1),
|
||||
# lifespan=Duration(nanoseconds=500_000_000), # 500ms
|
||||
# liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
|
||||
|
||||
@@ -6,28 +6,21 @@ from rclpy.duration import Duration
|
||||
|
||||
import signal
|
||||
import time
|
||||
import atexit
|
||||
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import glob
|
||||
import pwd
|
||||
import grp
|
||||
from math import copysign
|
||||
|
||||
from std_srvs.srv import Trigger
|
||||
from std_msgs.msg import Header
|
||||
from geometry_msgs.msg import Twist, TwistStamped
|
||||
from control_msgs.msg import JointJog
|
||||
from std_msgs.msg import String
|
||||
from geometry_msgs.msg import Twist
|
||||
from astra_msgs.msg import CoreControl, ArmManual, BioControl
|
||||
from astra_msgs.msg import CoreCtrlState
|
||||
|
||||
import warnings
|
||||
|
||||
# Literally headless
|
||||
warnings.filterwarnings(
|
||||
"ignore",
|
||||
message="Your system is avx2 capable but pygame was not built with support for it.",
|
||||
)
|
||||
|
||||
import pygame
|
||||
|
||||
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
|
||||
@@ -41,44 +34,26 @@ CORE_STOP_TWIST_MSG = Twist() # "
|
||||
ARM_STOP_MSG = ArmManual() # "
|
||||
BIO_STOP_MSG = BioControl() # "
|
||||
|
||||
|
||||
control_qos = qos.QoSProfile(
|
||||
history=qos.QoSHistoryPolicy.KEEP_LAST,
|
||||
# history=qos.QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=2,
|
||||
reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
|
||||
durability=qos.QoSDurabilityPolicy.VOLATILE,
|
||||
# reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
|
||||
# durability=qos.QoSDurabilityPolicy.VOLATILE,
|
||||
# deadline=Duration(seconds=1),
|
||||
# lifespan=Duration(nanoseconds=500_000_000), # 500ms
|
||||
# liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
|
||||
# liveliness_lease_duration=Duration(seconds=5),
|
||||
)
|
||||
|
||||
|
||||
STICK_DEADZONE = float(os.getenv("STICK_DEADZONE", "0.05"))
|
||||
ARM_DEADZONE = float(os.getenv("ARM_DEADZONE", "0.2"))
|
||||
CORE_MODE = "twist" # "twist" or "duty"
|
||||
|
||||
|
||||
class Headless(Node):
|
||||
# Every non-fixed joint defined in Arm's URDF
|
||||
# Used for JointState and JointJog messsages
|
||||
all_joint_names = [
|
||||
"axis_0_joint",
|
||||
"axis_1_joint",
|
||||
"axis_2_joint",
|
||||
"axis_3_joint",
|
||||
"wrist_yaw_joint",
|
||||
"wrist_roll_joint",
|
||||
"ef_gripper_left_joint",
|
||||
]
|
||||
|
||||
def __init__(self):
|
||||
# Initialize pygame first
|
||||
pygame.init()
|
||||
pygame.joystick.init()
|
||||
super().__init__("headless_node")
|
||||
|
||||
##################################################
|
||||
# Preamble
|
||||
super().__init__("headless")
|
||||
|
||||
# Wait for anchor to start
|
||||
pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
|
||||
@@ -130,119 +105,37 @@ class Headless(Node):
|
||||
break
|
||||
id += 1
|
||||
|
||||
##################################################
|
||||
# Parameters
|
||||
self.create_timer(0.15, self.send_controls)
|
||||
|
||||
self.declare_parameter("use_old_topics", True)
|
||||
self.use_old_topics = (
|
||||
self.get_parameter("use_old_topics").get_parameter_value().bool_value
|
||||
self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
|
||||
self.arm_publisher = self.create_publisher(ArmManual, "/arm/control/manual", 2)
|
||||
self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
|
||||
|
||||
self.core_twist_pub_ = self.create_publisher(
|
||||
Twist, "/core/twist", qos_profile=control_qos
|
||||
)
|
||||
|
||||
self.declare_parameter("use_bio", False)
|
||||
self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
|
||||
|
||||
self.declare_parameter("use_arm_ik", False)
|
||||
self.use_arm_ik = (
|
||||
self.get_parameter("use_arm_ik").get_parameter_value().bool_value
|
||||
self.core_state_pub_ = self.create_publisher(
|
||||
CoreCtrlState, "/core/control/state", qos_profile=control_qos
|
||||
)
|
||||
|
||||
# NOTE: only applicable if use_old_topics == True
|
||||
self.declare_parameter("use_new_arm_manual_scheme", True)
|
||||
self.use_new_arm_manual_scheme = (
|
||||
self.get_parameter("use_new_arm_manual_scheme")
|
||||
.get_parameter_value()
|
||||
.bool_value
|
||||
)
|
||||
|
||||
# Check parameter validity
|
||||
if self.use_arm_ik and self.use_old_topics:
|
||||
self.get_logger().fatal("Old topics do not support arm IK control.")
|
||||
sys.exit(1)
|
||||
if not self.use_new_arm_manual_scheme and not self.use_old_topics:
|
||||
self.get_logger().warn(
|
||||
f"New arm manual does not support old control scheme. Defaulting to new scheme."
|
||||
)
|
||||
|
||||
self.ctrl_mode = "core" # Start in core mode
|
||||
self.core_brake_mode = False
|
||||
self.core_max_duty = 0.5 # Default max duty cycle (walking speed)
|
||||
|
||||
##################################################
|
||||
# Old Topics
|
||||
|
||||
if self.use_old_topics:
|
||||
self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
|
||||
self.arm_publisher = self.create_publisher(
|
||||
ArmManual, "/arm/control/manual", 2
|
||||
)
|
||||
self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
|
||||
|
||||
##################################################
|
||||
# New Topics
|
||||
|
||||
if not self.use_old_topics:
|
||||
self.core_twist_pub_ = self.create_publisher(
|
||||
Twist, "/core/twist", qos_profile=control_qos
|
||||
)
|
||||
self.core_state_pub_ = self.create_publisher(
|
||||
CoreCtrlState, "/core/control/state", qos_profile=control_qos
|
||||
)
|
||||
|
||||
self.arm_manual_pub_ = self.create_publisher(
|
||||
JointJog, "/arm/manual_new", qos_profile=control_qos
|
||||
)
|
||||
|
||||
self.arm_ik_twist_publisher = self.create_publisher(
|
||||
TwistStamped, "/servo_node/delta_twist_cmds", qos_profile=control_qos
|
||||
)
|
||||
self.arm_ik_jointjog_publisher = self.create_publisher(
|
||||
JointJog, "/servo_node/delta_joint_cmds", qos_profile=control_qos
|
||||
)
|
||||
|
||||
# TODO: add new bio topics
|
||||
|
||||
##################################################
|
||||
# Timers
|
||||
|
||||
self.create_timer(0.1, self.send_controls)
|
||||
|
||||
##################################################
|
||||
# Services
|
||||
|
||||
# If using IK control, we have to "start" the servo node to enable it to accept commands
|
||||
self.servo_start_client = None
|
||||
if self.use_arm_ik:
|
||||
self.get_logger().info("Starting servo node for IK control...")
|
||||
self.servo_start_client = self.create_client(
|
||||
Trigger, "/servo_node/start_servo"
|
||||
)
|
||||
timeout_counter = 0
|
||||
while not self.servo_start_client.wait_for_service(timeout_sec=1.0):
|
||||
self.get_logger().info("Waiting for servo_node/start_servo service...")
|
||||
timeout_counter += 1
|
||||
if timeout_counter >= 10:
|
||||
self.get_logger().error(
|
||||
"Servo's start service not available. IK control will not work."
|
||||
)
|
||||
break
|
||||
if self.servo_start_client.service_is_ready():
|
||||
self.servo_start_client.call_async(Trigger.Request())
|
||||
|
||||
# Rumble when node is ready (returns False if rumble not supported)
|
||||
self.gamepad.rumble(0.7, 0.8, 150)
|
||||
|
||||
def stop_all(self):
|
||||
if self.use_old_topics:
|
||||
self.core_publisher.publish(CORE_STOP_MSG)
|
||||
self.arm_publisher.publish(ARM_STOP_MSG)
|
||||
self.bio_publisher.publish(BIO_STOP_MSG)
|
||||
else:
|
||||
self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
|
||||
if self.use_arm_ik:
|
||||
self.arm_ik_twist_publisher.publish(self.arm_ik_twist_stop_msg())
|
||||
else:
|
||||
self.arm_manual_pub_.publish(self.arm_manual_stop_msg())
|
||||
# TODO: add bio here after implementing new topics
|
||||
def run(self):
|
||||
# This thread makes all the update processes run in the background
|
||||
thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
|
||||
thread.start()
|
||||
|
||||
try:
|
||||
while rclpy.ok():
|
||||
self.send_controls()
|
||||
time.sleep(0.1) # Small delay to avoid CPU hogging
|
||||
except KeyboardInterrupt:
|
||||
sys.exit(0)
|
||||
|
||||
def send_controls(self):
|
||||
"""Read the gamepad state and publish control messages"""
|
||||
@@ -254,8 +147,10 @@ class Headless(Node):
|
||||
# Check if controller is still connected
|
||||
if pygame.joystick.get_count() != self.num_gamepads:
|
||||
print("Gamepad disconnected. Exiting...")
|
||||
# Stop the rover if controller disconnected
|
||||
self.stop_all()
|
||||
# Send one last zero control message
|
||||
self.core_publisher.publish(CORE_STOP_MSG)
|
||||
self.arm_publisher.publish(ARM_STOP_MSG)
|
||||
self.bio_publisher.publish(BIO_STOP_MSG)
|
||||
self.get_logger().info("Final stop commands sent. Shutting down.")
|
||||
# Clean up
|
||||
pygame.quit()
|
||||
@@ -271,51 +166,20 @@ class Headless(Node):
|
||||
new_ctrl_mode = "core"
|
||||
|
||||
if new_ctrl_mode != self.ctrl_mode:
|
||||
self.stop_all()
|
||||
self.gamepad.rumble(0.6, 0.7, 75)
|
||||
self.ctrl_mode = new_ctrl_mode
|
||||
self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
|
||||
if self.ctrl_mode == "arm" and self.use_bio:
|
||||
self.get_logger().warning("NOTE: Using bio instead of arm.")
|
||||
|
||||
# Actually send the controls
|
||||
if self.ctrl_mode == "core":
|
||||
self.send_core()
|
||||
if self.use_old_topics:
|
||||
if self.use_bio:
|
||||
self.bio_publisher.publish(BIO_STOP_MSG)
|
||||
else:
|
||||
self.arm_publisher.publish(ARM_STOP_MSG)
|
||||
# New topics shouldn't need to constantly send zeroes imo
|
||||
else:
|
||||
if self.use_bio:
|
||||
self.send_bio()
|
||||
else:
|
||||
self.send_arm()
|
||||
if self.use_old_topics:
|
||||
self.core_publisher.publish(CORE_STOP_MSG)
|
||||
# Ditto
|
||||
|
||||
def send_core(self):
|
||||
# Collect controller state
|
||||
left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
|
||||
left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
|
||||
left_trigger = stick_deadzone(self.gamepad.get_axis(2))
|
||||
right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
|
||||
right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
|
||||
right_trigger = stick_deadzone(self.gamepad.get_axis(5))
|
||||
button_a = self.gamepad.get_button(0)
|
||||
button_b = self.gamepad.get_button(1)
|
||||
button_x = self.gamepad.get_button(2)
|
||||
button_y = self.gamepad.get_button(3)
|
||||
left_bumper = self.gamepad.get_button(4)
|
||||
right_bumper = self.gamepad.get_button(5)
|
||||
dpad_input = self.gamepad.get_hat(0)
|
||||
|
||||
if self.use_old_topics:
|
||||
# CORE
|
||||
if self.ctrl_mode == "core" and CORE_MODE == "duty":
|
||||
input = CoreControl()
|
||||
input.max_speed = 90
|
||||
|
||||
# Collect controller state
|
||||
left_stick_y = deadzone(self.gamepad.get_axis(1))
|
||||
right_stick_y = deadzone(self.gamepad.get_axis(4))
|
||||
right_trigger = deadzone(self.gamepad.get_axis(5))
|
||||
|
||||
# Right wheels
|
||||
input.right_stick = float(round(-1 * right_stick_y, 2))
|
||||
|
||||
@@ -330,10 +194,19 @@ class Headless(Node):
|
||||
self.get_logger().info(f"[Ctrl] {output}")
|
||||
|
||||
self.core_publisher.publish(input)
|
||||
self.arm_publisher.publish(ARM_STOP_MSG)
|
||||
# self.bio_publisher.publish(BIO_STOP_MSG)
|
||||
|
||||
else: # New topics
|
||||
elif self.ctrl_mode == "core" and CORE_MODE == "twist":
|
||||
input = Twist()
|
||||
|
||||
# Collect controller state
|
||||
left_stick_y = deadzone(self.gamepad.get_axis(1))
|
||||
right_stick_x = deadzone(self.gamepad.get_axis(3))
|
||||
button_a = self.gamepad.get_button(0)
|
||||
left_bumper = self.gamepad.get_button(4)
|
||||
right_bumper = self.gamepad.get_button(5)
|
||||
|
||||
# Forward/back and Turn
|
||||
input.linear.x = -1.0 * left_stick_y
|
||||
input.angular.z = -1.0 * copysign(
|
||||
@@ -342,6 +215,8 @@ class Headless(Node):
|
||||
|
||||
# Publish
|
||||
self.core_twist_pub_.publish(input)
|
||||
self.arm_publisher.publish(ARM_STOP_MSG)
|
||||
# self.bio_publisher.publish(BIO_STOP_MSG)
|
||||
self.get_logger().info(
|
||||
f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
|
||||
)
|
||||
@@ -366,310 +241,91 @@ class Headless(Node):
|
||||
state_msg = CoreCtrlState()
|
||||
state_msg.brake_mode = bool(self.core_brake_mode)
|
||||
state_msg.max_duty = float(self.core_max_duty)
|
||||
|
||||
self.core_state_pub_.publish(state_msg)
|
||||
self.get_logger().info(
|
||||
f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}"
|
||||
)
|
||||
|
||||
def send_arm(self):
|
||||
# Collect controller state
|
||||
left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
|
||||
left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
|
||||
left_trigger = stick_deadzone(self.gamepad.get_axis(2))
|
||||
right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
|
||||
right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
|
||||
right_trigger = stick_deadzone(self.gamepad.get_axis(5))
|
||||
button_a = self.gamepad.get_button(0)
|
||||
button_b = self.gamepad.get_button(1)
|
||||
button_x = self.gamepad.get_button(2)
|
||||
button_y = self.gamepad.get_button(3)
|
||||
left_bumper = self.gamepad.get_button(4)
|
||||
right_bumper = self.gamepad.get_button(5)
|
||||
dpad_input = self.gamepad.get_hat(0)
|
||||
|
||||
# OLD MANUAL
|
||||
# ==========
|
||||
|
||||
if not self.use_arm_ik and self.use_old_topics:
|
||||
# ARM and BIO
|
||||
if self.ctrl_mode == "arm":
|
||||
arm_input = ArmManual()
|
||||
|
||||
# OLD ARM MANUAL CONTROL SCHEME
|
||||
if not self.use_new_arm_manual_scheme:
|
||||
# EF Grippers
|
||||
if left_trigger > 0 and right_trigger > 0:
|
||||
arm_input.gripper = 0
|
||||
elif left_trigger > 0:
|
||||
arm_input.gripper = -1
|
||||
elif right_trigger > 0:
|
||||
arm_input.gripper = 1
|
||||
# Collect controller state
|
||||
left_stick_x = deadzone(self.gamepad.get_axis(0))
|
||||
left_stick_y = deadzone(self.gamepad.get_axis(1))
|
||||
left_trigger = deadzone(self.gamepad.get_axis(2))
|
||||
right_stick_x = deadzone(self.gamepad.get_axis(3))
|
||||
right_stick_y = deadzone(self.gamepad.get_axis(4))
|
||||
right_trigger = deadzone(self.gamepad.get_axis(5))
|
||||
right_bumper = self.gamepad.get_button(5)
|
||||
dpad_input = self.gamepad.get_hat(0)
|
||||
|
||||
# Axis 0
|
||||
if dpad_input[0] == 1:
|
||||
arm_input.axis0 = 1
|
||||
elif dpad_input[0] == -1:
|
||||
arm_input.axis0 = -1
|
||||
|
||||
if right_bumper: # Control end effector
|
||||
|
||||
# Effector yaw
|
||||
if left_stick_x > 0:
|
||||
arm_input.effector_yaw = 1
|
||||
elif left_stick_x < 0:
|
||||
arm_input.effector_yaw = -1
|
||||
|
||||
# Effector roll
|
||||
if right_stick_x > 0:
|
||||
arm_input.effector_roll = 1
|
||||
elif right_stick_x < 0:
|
||||
arm_input.effector_roll = -1
|
||||
|
||||
else: # Control arm axis
|
||||
|
||||
# Axis 1
|
||||
if abs(left_stick_x) > 0.15:
|
||||
arm_input.axis1 = round(left_stick_x)
|
||||
|
||||
# Axis 2
|
||||
if abs(left_stick_y) > 0.15:
|
||||
arm_input.axis2 = -1 * round(left_stick_y)
|
||||
|
||||
# Axis 3
|
||||
if abs(right_stick_y) > 0.15:
|
||||
arm_input.axis3 = -1 * round(right_stick_y)
|
||||
|
||||
# NEW ARM MANUAL CONTROL SCHEME
|
||||
if self.use_new_arm_manual_scheme:
|
||||
# Right stick: EF yaw and axis 3
|
||||
# Left stick: axis 1 and 2
|
||||
# D-pad: axis 0 and _
|
||||
# Triggers: EF grippers
|
||||
# Bumpers: EF roll
|
||||
# A: brake
|
||||
# B: linear actuator in
|
||||
# X: _
|
||||
# Y: linear actuator out
|
||||
|
||||
# Right stick: EF yaw and axis 3
|
||||
arm_input.effector_yaw = stick_to_arm_direction(right_stick_x)
|
||||
arm_input.axis3 = -1 * stick_to_arm_direction(right_stick_y)
|
||||
|
||||
# Left stick: axis 1 and 2
|
||||
arm_input.axis1 = stick_to_arm_direction(left_stick_x)
|
||||
arm_input.axis2 = -1 * stick_to_arm_direction(left_stick_y)
|
||||
|
||||
# D-pad: axis 0 and _
|
||||
arm_input.axis0 = int(dpad_input[0])
|
||||
|
||||
# Triggers: EF Grippers
|
||||
if left_trigger > 0 and right_trigger > 0:
|
||||
arm_input.gripper = 0
|
||||
elif left_trigger > 0:
|
||||
arm_input.gripper = -1
|
||||
elif right_trigger > 0:
|
||||
arm_input.gripper = 1
|
||||
|
||||
# Bumpers: EF roll
|
||||
if left_bumper > 0 and right_bumper > 0:
|
||||
arm_input.effector_roll = 0
|
||||
elif left_bumper > 0:
|
||||
arm_input.effector_roll = -1
|
||||
elif right_bumper > 0:
|
||||
arm_input.effector_roll = 1
|
||||
|
||||
# A: brake
|
||||
if button_a:
|
||||
arm_input.brake = True
|
||||
|
||||
# Y: linear actuator
|
||||
if button_y and not button_b:
|
||||
arm_input.linear_actuator = 1
|
||||
elif button_b and not button_y:
|
||||
arm_input.linear_actuator = -1
|
||||
else:
|
||||
arm_input.linear_actuator = 0
|
||||
|
||||
self.arm_publisher.publish(arm_input)
|
||||
|
||||
# NEW MANUAL
|
||||
# ==========
|
||||
|
||||
elif not self.use_arm_ik and not self.use_old_topics:
|
||||
arm_input = JointJog()
|
||||
arm_input.header.frame_id = "base_link"
|
||||
arm_input.header.stamp = self.get_clock().now().to_msg()
|
||||
arm_input.joint_names = self.all_joint_names
|
||||
arm_input.velocities = [0.0] * len(self.all_joint_names)
|
||||
|
||||
# Right stick: EF yaw and axis 3
|
||||
# Left stick: axis 1 and 2
|
||||
# D-pad: axis 0 and _
|
||||
# Triggers: EF grippers
|
||||
# Bumpers: EF roll
|
||||
# A: brake
|
||||
# B: linear actuator in
|
||||
# X: _
|
||||
# Y: linear actuator out
|
||||
|
||||
# Right stick: EF yaw and axis 3
|
||||
arm_input.velocities[self.all_joint_names.index("wrist_yaw_joint")] = float(
|
||||
stick_to_arm_direction(right_stick_x)
|
||||
)
|
||||
arm_input.velocities[self.all_joint_names.index("axis_3_joint")] = float(
|
||||
stick_to_arm_direction(right_stick_y)
|
||||
)
|
||||
|
||||
# Left stick: axis 1 and 2
|
||||
arm_input.velocities[self.all_joint_names.index("axis_1_joint")] = float(
|
||||
stick_to_arm_direction(left_stick_x)
|
||||
)
|
||||
arm_input.velocities[self.all_joint_names.index("axis_2_joint")] = float(
|
||||
stick_to_arm_direction(left_stick_y)
|
||||
)
|
||||
|
||||
# D-pad: axis 0 and _
|
||||
arm_input.velocities[self.all_joint_names.index("axis_0_joint")] = float(
|
||||
dpad_input[0]
|
||||
)
|
||||
|
||||
# Triggers: EF Grippers
|
||||
# EF Grippers
|
||||
if left_trigger > 0 and right_trigger > 0:
|
||||
arm_input.velocities[
|
||||
self.all_joint_names.index("ef_gripper_left_joint")
|
||||
] = 0.0
|
||||
arm_input.gripper = 0
|
||||
elif left_trigger > 0:
|
||||
arm_input.velocities[
|
||||
self.all_joint_names.index("ef_gripper_left_joint")
|
||||
] = -1.0
|
||||
arm_input.gripper = -1
|
||||
elif right_trigger > 0:
|
||||
arm_input.velocities[
|
||||
self.all_joint_names.index("ef_gripper_left_joint")
|
||||
] = 1.0
|
||||
arm_input.gripper = 1
|
||||
|
||||
# Bumpers: EF roll
|
||||
arm_input.velocities[self.all_joint_names.index("wrist_roll_joint")] = (
|
||||
right_bumper - left_bumper
|
||||
)
|
||||
# Axis 0
|
||||
if dpad_input[0] == 1:
|
||||
arm_input.axis0 = 1
|
||||
elif dpad_input[0] == -1:
|
||||
arm_input.axis0 = -1
|
||||
|
||||
# A: brake
|
||||
# TODO: Brake mode
|
||||
if right_bumper: # Control end effector
|
||||
|
||||
# Y: linear actuator
|
||||
# TODO: linear actuator
|
||||
# Effector yaw
|
||||
if left_stick_x > 0:
|
||||
arm_input.effector_yaw = 1
|
||||
elif left_stick_x < 0:
|
||||
arm_input.effector_yaw = -1
|
||||
|
||||
self.arm_manual_pub_.publish(arm_input)
|
||||
# Effector roll
|
||||
if right_stick_x > 0:
|
||||
arm_input.effector_roll = 1
|
||||
elif right_stick_x < 0:
|
||||
arm_input.effector_roll = -1
|
||||
|
||||
# IK (ONLY NEW)
|
||||
# =============
|
||||
else: # Control arm axis
|
||||
|
||||
elif self.use_arm_ik:
|
||||
arm_twist = TwistStamped()
|
||||
arm_twist.header.frame_id = "base_link"
|
||||
arm_twist.header.stamp = self.get_clock().now().to_msg()
|
||||
arm_jointjog = JointJog()
|
||||
arm_jointjog.header.frame_id = "base_link"
|
||||
arm_jointjog.header.stamp = self.get_clock().now().to_msg()
|
||||
# Axis 1
|
||||
if abs(left_stick_x) > 0.15:
|
||||
arm_input.axis1 = round(left_stick_x)
|
||||
|
||||
# Right stick: linear y and linear x
|
||||
# Left stick: angular z and linear z
|
||||
# D-pad: angular y and _
|
||||
# Triggers: EF grippers
|
||||
# Bumpers: angular x
|
||||
# A: brake
|
||||
# B: IK mode
|
||||
# X: manual mode
|
||||
# Y: linear actuator
|
||||
# Axis 2
|
||||
if abs(left_stick_y) > 0.15:
|
||||
arm_input.axis2 = -1 * round(left_stick_y)
|
||||
|
||||
# Right stick: linear y and linear x
|
||||
arm_twist.twist.linear.y = float(right_stick_x)
|
||||
arm_twist.twist.linear.x = float(right_stick_y)
|
||||
# Axis 3
|
||||
if abs(right_stick_y) > 0.15:
|
||||
arm_input.axis3 = -1 * round(right_stick_y)
|
||||
|
||||
# Left stick: angular z and linear z
|
||||
arm_twist.twist.angular.z = float(-1 * left_stick_x)
|
||||
arm_twist.twist.linear.z = float(-1 * left_stick_y)
|
||||
# D-pad: angular y and _
|
||||
arm_twist.twist.angular.y = (
|
||||
float(0)
|
||||
if dpad_input[0] == 0
|
||||
else float(-1 * copysign(0.75, dpad_input[0]))
|
||||
)
|
||||
|
||||
# Triggers: EF Grippers
|
||||
if left_trigger > 0 or right_trigger > 0:
|
||||
arm_jointjog.joint_names.append("ef_gripper_left_joint") # type: ignore
|
||||
arm_jointjog.velocities.append(float(right_trigger - left_trigger))
|
||||
|
||||
# Bumpers: angular x
|
||||
if left_bumper > 0 and right_bumper > 0:
|
||||
arm_twist.twist.angular.x = float(0)
|
||||
elif left_bumper > 0:
|
||||
arm_twist.twist.angular.x = float(1)
|
||||
elif right_bumper > 0:
|
||||
arm_twist.twist.angular.x = float(-1)
|
||||
|
||||
self.arm_ik_twist_publisher.publish(arm_twist)
|
||||
# self.arm_ik_jointjog_publisher.publish(arm_jointjog) # TODO: Figure this shit out
|
||||
|
||||
def send_bio(self):
|
||||
# Collect controller state
|
||||
left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
|
||||
left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
|
||||
left_trigger = stick_deadzone(self.gamepad.get_axis(2))
|
||||
right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
|
||||
right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
|
||||
right_trigger = stick_deadzone(self.gamepad.get_axis(5))
|
||||
button_a = self.gamepad.get_button(0)
|
||||
button_b = self.gamepad.get_button(1)
|
||||
button_x = self.gamepad.get_button(2)
|
||||
button_y = self.gamepad.get_button(3)
|
||||
left_bumper = self.gamepad.get_button(4)
|
||||
right_bumper = self.gamepad.get_button(5)
|
||||
dpad_input = self.gamepad.get_hat(0)
|
||||
|
||||
if self.use_old_topics:
|
||||
# BIO
|
||||
bio_input = BioControl(
|
||||
bio_arm=int(left_stick_y * -100),
|
||||
drill_arm=int(round(right_stick_y) * -100),
|
||||
)
|
||||
|
||||
# Drill motor (FAERIE)
|
||||
if left_trigger > 0 or right_trigger > 0:
|
||||
if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
|
||||
bio_input.drill = int(
|
||||
30 * (right_trigger - left_trigger)
|
||||
) # Max duty cycle 30%
|
||||
|
||||
self.bio_publisher.publish(bio_input)
|
||||
|
||||
else:
|
||||
pass # TODO: implement new bio control topics
|
||||
|
||||
def arm_manual_stop_msg(self):
|
||||
return JointJog(
|
||||
header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()),
|
||||
joint_names=self.all_joint_names,
|
||||
velocities=[0.0] * len(self.all_joint_names),
|
||||
)
|
||||
|
||||
def arm_ik_twist_stop_msg(self):
|
||||
return TwistStamped(
|
||||
header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
|
||||
)
|
||||
self.core_publisher.publish(CORE_STOP_MSG)
|
||||
self.arm_publisher.publish(arm_input)
|
||||
# self.bio_publisher.publish(bio_input)
|
||||
|
||||
|
||||
def stick_deadzone(value: float, threshold=STICK_DEADZONE) -> float:
|
||||
def deadzone(value: float, threshold=0.05) -> float:
|
||||
"""Apply a deadzone to a joystick input so the motors don't sound angry"""
|
||||
if abs(value) < threshold:
|
||||
return 0
|
||||
return value
|
||||
|
||||
|
||||
def stick_to_arm_direction(value: float, threshold=ARM_DEADZONE) -> int:
|
||||
"""Apply a larger deadzone to a stick input and make digital/binary instead of analog"""
|
||||
if abs(value) < threshold:
|
||||
return 0
|
||||
return int(copysign(1, value))
|
||||
|
||||
|
||||
def is_user_in_group(group_name: str) -> bool:
|
||||
# Copied from https://zetcode.com/python/os-getgrouplist/
|
||||
try:
|
||||
@@ -688,26 +344,20 @@ def is_user_in_group(group_name: str) -> bool:
|
||||
return False
|
||||
|
||||
|
||||
def exit_handler(signum, frame):
|
||||
print("Caught SIGTERM. Exiting...")
|
||||
rclpy.try_shutdown()
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
|
||||
# Catch termination signals and exit cleanly
|
||||
signal.signal(signal.SIGTERM, exit_handler)
|
||||
|
||||
node = Headless()
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
print("Caught shutdown signal. Exiting...")
|
||||
finally:
|
||||
rclpy.try_shutdown()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
signal.signal(
|
||||
signal.SIGTERM, lambda signum, frame: sys.exit(0)
|
||||
) # Catch termination signals and exit cleanly
|
||||
main()
|
||||
|
||||
@@ -26,7 +26,7 @@ class LatencyTester : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
LatencyTester()
|
||||
: Node("latency_tester"), count_(0)
|
||||
: Node("latency_tester"), count_(0), target_mcu_("core")
|
||||
{
|
||||
publisher_ = this->create_publisher<std_msgs::msg::String>("/anchor/relay", 10);
|
||||
timer_ = this->create_wall_timer(
|
||||
@@ -35,8 +35,6 @@ public:
|
||||
"/anchor/debug",
|
||||
10,
|
||||
std::bind(&LatencyTester::response_callback, this, std::placeholders::_1));
|
||||
|
||||
target_mcu_ = this->declare_parameter<std::string>("target_mcu", "core");
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
80
src/servo_arm_twist_pkg/CMakeLists.txt
Normal file
80
src/servo_arm_twist_pkg/CMakeLists.txt
Normal file
@@ -0,0 +1,80 @@
|
||||
cmake_minimum_required(VERSION 3.22)
|
||||
project(servo_arm_twist_pkg)
|
||||
|
||||
# C++ Libraries #################################################
|
||||
|
||||
# Core C++ library for calculations and collision checking.
|
||||
# Provides interface used by the component node.
|
||||
set(SERVO_LIB_NAME servo_arm_twist_lib)
|
||||
|
||||
# Pose Tracking
|
||||
set(POSE_TRACKING pose_tracking)
|
||||
|
||||
# Component Nodes (Shared libraries) ############################
|
||||
set(SERVO_COMPONENT_NODE servo_node)
|
||||
set(SERVO_CONTROLLER_INPUT servo_controller_input)
|
||||
|
||||
# Executable Nodes ##############################################
|
||||
set(SERVO_NODE_MAIN_NAME servo_node_main)
|
||||
set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
|
||||
set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
|
||||
|
||||
#################################################################
|
||||
|
||||
# Common cmake code applied to all moveit packages
|
||||
find_package(moveit_common REQUIRED)
|
||||
moveit_package()
|
||||
|
||||
set(THIS_PACKAGE_INCLUDE_DEPENDS
|
||||
control_msgs
|
||||
control_toolbox
|
||||
geometry_msgs
|
||||
moveit_core
|
||||
moveit_msgs
|
||||
moveit_ros_planning
|
||||
pluginlib
|
||||
rclcpp
|
||||
rclcpp_components
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
std_srvs
|
||||
tf2_eigen
|
||||
trajectory_msgs
|
||||
)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(eigen3_cmake_module REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
find_package(${Dependency} REQUIRED)
|
||||
endforeach()
|
||||
|
||||
#####################
|
||||
## Component Nodes ##
|
||||
#####################
|
||||
|
||||
# Add executable for using a controller
|
||||
add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joystick_twist.cpp)
|
||||
ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "servo_arm_twist_pkg::JoyToServoPub")
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# Install Libraries
|
||||
install(
|
||||
TARGETS
|
||||
${SERVO_CONTROLLER_INPUT}
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
LIBRARY DESTINATION lib
|
||||
ARCHIVE DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
INCLUDES DESTINATION include
|
||||
)
|
||||
|
||||
# Install Binaries
|
||||
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
||||
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
|
||||
ament_package()
|
||||
3
src/servo_arm_twist_pkg/README.md
Normal file
3
src/servo_arm_twist_pkg/README.md
Normal file
@@ -0,0 +1,3 @@
|
||||
# Moveit Servo
|
||||
|
||||
See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot.
|
||||
58
src/servo_arm_twist_pkg/package.xml
Normal file
58
src/servo_arm_twist_pkg/package.xml
Normal file
@@ -0,0 +1,58 @@
|
||||
<?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>servo_arm_twist_pkg</name>
|
||||
<version>2.5.9</version>
|
||||
<description>Provides real-time manipulator Cartesian and joint servoing.</description>
|
||||
<maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
|
||||
<maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
|
||||
<maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
|
||||
<maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
|
||||
|
||||
<license>BSD 3-Clause</license>
|
||||
|
||||
<url type="website">https://ros-planning.github.io/moveit_tutorials</url>
|
||||
|
||||
<author>Brian O'Neil</author>
|
||||
<author email="andyz@utexas.edu">Andy Zelenak</author>
|
||||
<author>Blake Anderson</author>
|
||||
<author email="alex@machinekoder.com">Alexander Rössler</author>
|
||||
<author email="tyler@picknik.ai">Tyler Weaver</author>
|
||||
<author email="adam.pettinger@utexas.edu">Adam Pettinger</author>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>moveit_common</depend>
|
||||
|
||||
<depend>control_msgs</depend>
|
||||
<depend>control_toolbox</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>moveit_msgs</depend>
|
||||
<depend>moveit_core</depend>
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>tf2_eigen</depend>
|
||||
<depend>trajectory_msgs</depend>
|
||||
|
||||
<exec_depend>gripper_controllers</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
<exec_depend>joint_trajectory_controller</exec_depend>
|
||||
<exec_depend>joy</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>moveit_configs_utils</exec_depend>
|
||||
<exec_depend>launch_param_builder</exec_depend>
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>controller_manager</test_depend>
|
||||
<test_depend>ros_testing</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
271
src/servo_arm_twist_pkg/src/joystick_twist.cpp
Normal file
271
src/servo_arm_twist_pkg/src/joystick_twist.cpp
Normal file
@@ -0,0 +1,271 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2020, PickNik Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of PickNik Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Title : joystick_servo_example.cpp
|
||||
* Project : servo_arm_twist_pkg
|
||||
* Created : 08/07/2020
|
||||
* Author : Adam Pettinger
|
||||
*/
|
||||
|
||||
#include <sensor_msgs/msg/joy.hpp>
|
||||
#include <geometry_msgs/msg/twist_stamped.hpp>
|
||||
#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/time.hpp>
|
||||
#include <rclcpp/utilities.hpp>
|
||||
#include <thread>
|
||||
|
||||
// We'll just set up parameters here
|
||||
const std::string JOY_TOPIC = "/joy";
|
||||
const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
|
||||
const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
|
||||
const std::string EEF_FRAME_ID = "End_Effector";
|
||||
const std::string BASE_FRAME_ID = "base_link";
|
||||
|
||||
// Enums for button names -> axis/button array index
|
||||
// For XBOX 1 controller
|
||||
enum Axis
|
||||
{
|
||||
LEFT_STICK_X = 0,
|
||||
LEFT_STICK_Y = 1,
|
||||
LEFT_TRIGGER = 2,
|
||||
RIGHT_STICK_X = 3,
|
||||
RIGHT_STICK_Y = 4,
|
||||
RIGHT_TRIGGER = 5,
|
||||
D_PAD_X = 6,
|
||||
D_PAD_Y = 7
|
||||
};
|
||||
enum Button
|
||||
{
|
||||
A = 0,
|
||||
B = 1,
|
||||
X = 2,
|
||||
Y = 3,
|
||||
LEFT_BUMPER = 4,
|
||||
RIGHT_BUMPER = 5,
|
||||
CHANGE_VIEW = 6,
|
||||
MENU = 7,
|
||||
HOME = 8,
|
||||
LEFT_STICK_CLICK = 9,
|
||||
RIGHT_STICK_CLICK = 10
|
||||
};
|
||||
|
||||
// Some axes have offsets (e.g. the default trigger position is 1.0 not 0)
|
||||
// This will map the default values for the axes
|
||||
std::map<Axis, double> AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
|
||||
std::map<Button, double> BUTTON_DEFAULTS;
|
||||
|
||||
// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2
|
||||
// functions
|
||||
/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message
|
||||
* @param axes The vector of continuous controller joystick axes
|
||||
* @param buttons The vector of discrete controller button values
|
||||
* @param twist A TwistStamped message to update in prep for publishing
|
||||
* @param joint A JointJog message to update in prep for publishing
|
||||
* @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)
|
||||
{
|
||||
// // Give joint jogging priority because it is only buttons
|
||||
// // If any joint jog command is requested, we are only publishing joint commands
|
||||
// if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
|
||||
// {
|
||||
// // Map the D_PAD to the proximal joints
|
||||
// joint->joint_names.push_back("panda_joint1");
|
||||
// joint->velocities.push_back(axes[D_PAD_X]);
|
||||
// joint->joint_names.push_back("panda_joint2");
|
||||
// joint->velocities.push_back(axes[D_PAD_Y]);
|
||||
|
||||
// // Map the diamond to the distal joints
|
||||
// joint->joint_names.push_back("panda_joint7");
|
||||
// joint->velocities.push_back(buttons[B] - buttons[X]);
|
||||
// joint->joint_names.push_back("panda_joint6");
|
||||
// joint->velocities.push_back(buttons[Y] - buttons[A]);
|
||||
// return false;
|
||||
// }
|
||||
|
||||
// The bread and butter: map buttons to twist commands
|
||||
twist->twist.linear.z = axes[RIGHT_STICK_Y];
|
||||
twist->twist.linear.y = axes[RIGHT_STICK_X];
|
||||
|
||||
double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER));
|
||||
double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER));
|
||||
twist->twist.linear.x = lin_x_right + lin_x_left;
|
||||
|
||||
twist->twist.angular.y = axes[LEFT_STICK_Y];
|
||||
twist->twist.angular.x = axes[LEFT_STICK_X];
|
||||
|
||||
double roll_positive = buttons[RIGHT_BUMPER];
|
||||
double roll_negative = -1 * (buttons[LEFT_BUMPER]);
|
||||
twist->twist.angular.z = roll_positive + roll_negative;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller
|
||||
* @param frame_name Set the command frame to this
|
||||
* @param buttons The vector of discrete controller button values
|
||||
*/
|
||||
void updateCmdFrame(std::string& frame_name, const std::vector<int>& buttons)
|
||||
{
|
||||
if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID)
|
||||
frame_name = BASE_FRAME_ID;
|
||||
else if (buttons[MENU] && frame_name == BASE_FRAME_ID)
|
||||
frame_name = EEF_FRAME_ID;
|
||||
}
|
||||
|
||||
namespace servo_arm_twist_pkg
|
||||
{
|
||||
class JoyToServoPub : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
JoyToServoPub(const rclcpp::NodeOptions& options)
|
||||
: Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID)
|
||||
{
|
||||
// Setup pub/sub
|
||||
joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
|
||||
JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
|
||||
[this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
|
||||
|
||||
twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
|
||||
joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
|
||||
// collision_pub_ =
|
||||
// this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", rclcpp::SystemDefaultsQoS());
|
||||
|
||||
// Create a service client to start the ServoNode
|
||||
servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
|
||||
servo_start_client_->wait_for_service(std::chrono::seconds(1));
|
||||
servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
|
||||
|
||||
// // Load the collision scene asynchronously
|
||||
// collision_pub_thread_ = std::thread([this]() {
|
||||
// rclcpp::sleep_for(std::chrono::seconds(3));
|
||||
// // Create collision object, in the way of servoing
|
||||
// moveit_msgs::msg::CollisionObject collision_object;
|
||||
// collision_object.header.frame_id = "panda_link0";
|
||||
// collision_object.id = "box";
|
||||
|
||||
// shape_msgs::msg::SolidPrimitive table_1;
|
||||
// table_1.type = table_1.BOX;
|
||||
// table_1.dimensions = { 0.4, 0.6, 0.03 };
|
||||
|
||||
// geometry_msgs::msg::Pose table_1_pose;
|
||||
// table_1_pose.position.x = 0.6;
|
||||
// table_1_pose.position.y = 0.0;
|
||||
// table_1_pose.position.z = 0.4;
|
||||
|
||||
// shape_msgs::msg::SolidPrimitive table_2;
|
||||
// table_2.type = table_2.BOX;
|
||||
// table_2.dimensions = { 0.6, 0.4, 0.03 };
|
||||
|
||||
// geometry_msgs::msg::Pose table_2_pose;
|
||||
// table_2_pose.position.x = 0.0;
|
||||
// table_2_pose.position.y = 0.5;
|
||||
// table_2_pose.position.z = 0.25;
|
||||
|
||||
// collision_object.primitives.push_back(table_1);
|
||||
// collision_object.primitive_poses.push_back(table_1_pose);
|
||||
// collision_object.primitives.push_back(table_2);
|
||||
// collision_object.primitive_poses.push_back(table_2_pose);
|
||||
// collision_object.operation = collision_object.ADD;
|
||||
|
||||
// moveit_msgs::msg::PlanningSceneWorld psw;
|
||||
// psw.collision_objects.push_back(collision_object);
|
||||
|
||||
// auto ps = std::make_unique<moveit_msgs::msg::PlanningScene>();
|
||||
// ps->world = psw;
|
||||
// ps->is_diff = true;
|
||||
// collision_pub_->publish(std::move(ps));
|
||||
// });
|
||||
}
|
||||
|
||||
// ~JoyToServoPub() override
|
||||
// {
|
||||
// if (collision_pub_thread_.joinable())
|
||||
// collision_pub_thread_.join();
|
||||
// }
|
||||
|
||||
void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg)
|
||||
{
|
||||
// Create the messages we might publish
|
||||
auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
|
||||
auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
|
||||
|
||||
// This call updates the frame for twist commands
|
||||
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))
|
||||
{
|
||||
// publish the TwistStamped
|
||||
twist_msg->header.frame_id = frame_to_publish_;
|
||||
twist_msg->header.stamp = this->now();
|
||||
twist_pub_->publish(std::move(twist_msg));
|
||||
}
|
||||
// else
|
||||
// {
|
||||
// // publish the JointJog
|
||||
// joint_msg->header.stamp = this->now();
|
||||
// joint_msg->header.frame_id = "panda_link3";
|
||||
// joint_pub_->publish(std::move(joint_msg));
|
||||
// }
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
|
||||
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
|
||||
rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr collision_pub_;
|
||||
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr servo_start_client_;
|
||||
|
||||
std::string frame_to_publish_;
|
||||
|
||||
// std::thread collision_pub_thread_;
|
||||
}; // class JoyToServoPub
|
||||
|
||||
} // namespace servo_arm_twist_pkg
|
||||
|
||||
// Register the component with class_loader
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(servo_arm_twist_pkg::JoyToServoPub)
|
||||
@@ -1,8 +0,0 @@
|
||||
{ pkgs, ... }:
|
||||
{
|
||||
projectRootFile = "flake.nix";
|
||||
programs = {
|
||||
nixfmt.enable = true;
|
||||
black.enable = true;
|
||||
};
|
||||
}
|
||||
Reference in New Issue
Block a user