mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-02-11 09:20:40 +00:00
Compare commits
35 Commits
v1.2
...
gazebo-doc
| Author | SHA1 | Date | |
|---|---|---|---|
| 4d84b5f8cc | |||
|
4a49069c2a | ||
|
|
d093c0b725 | ||
|
|
5d5f864cd7 | ||
|
|
d7fd133586 | ||
|
|
c107b82a8d | ||
|
|
b670bc2eda | ||
|
|
9fea136575 | ||
|
|
0fa2226529 | ||
|
|
3ebd2e29a3 | ||
|
|
9516e53f68 | ||
|
|
18a7b7e2ab | ||
|
|
e351e4c991 | ||
|
|
f735f7194e | ||
|
|
90e2aa5070 | ||
|
|
611ac90f54 | ||
|
|
01ea43968d | ||
|
|
4ce183773d | ||
|
|
3413615461 | ||
|
|
9125391de9 | ||
|
|
981b0b166c | ||
|
|
9471992d3b | ||
|
|
9579b64cb0 | ||
|
|
d92ca3ae5a | ||
|
|
d72a9a3b5e | ||
|
|
1b05202efa | ||
|
|
508fa8e2ae | ||
|
|
77bf59d5fd | ||
|
|
0d09c81802 | ||
|
|
fa10027e2d | ||
|
|
bb2dda02a2 | ||
|
|
c0d39aa3a6 | ||
|
|
6671f290e5 | ||
|
|
2db9b67ebc | ||
|
|
1281236b36 |
1
.gitignore
vendored
1
.gitignore
vendored
@@ -14,3 +14,4 @@ __pycache__/
|
||||
|
||||
#Direnv
|
||||
.direnv/
|
||||
.venv
|
||||
|
||||
6
.gitmodules
vendored
6
.gitmodules
vendored
@@ -1,6 +1,6 @@
|
||||
[submodule "src/ros2_interfaces_pkg"]
|
||||
path = src/ros2_interfaces_pkg
|
||||
url = ../ros2_interfaces_pkg
|
||||
[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
|
||||
|
||||
201
README.md
201
README.md
@@ -1,89 +1,148 @@
|
||||
# rover-ros2
|
||||
# ASTRA Rover ROS2 Packages
|
||||
|
||||
[](https://www.gnu.org/licenses/agpl-3.0)
|
||||
|
||||
Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation.
|
||||
Includes all main ROS2 packages for the rover. These are centrally located for modular rover operation.
|
||||
|
||||
You will use this package to launch any module-side ROS2 nodes.
|
||||
<br>
|
||||
You will use these packages to launch all rover-side ROS2 nodes.
|
||||
|
||||
## Software Pre-reqs
|
||||
## Table of Contents
|
||||
|
||||
An acting base station computer will need several things:
|
||||
- [Software Prerequisites](#software-prerequisites)
|
||||
- [Nix](#nix)
|
||||
- [ROS2 Humble + rosdep](#ros2-humble--rosdep)
|
||||
- [Running](#running)
|
||||
- [Testing Serial](#testing-serial)
|
||||
- [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
|
||||
- [Common Problems/Toubleshooting](#common-problemstroubleshooting)
|
||||
- [Packages](#packages)
|
||||
- [Maintainers](#maintainers)
|
||||
|
||||
* ROS2 Humble
|
||||
* Follow the standard ROS2 humble install process. Linux recommended.
|
||||
* https://docs.ros.org/en/humble/Installation.html
|
||||
* Colcon
|
||||
* `$ sudo apt update`
|
||||
* `$ sudo apt install python3-colcon-common-extensions`
|
||||
* Configured Static IP for Ubiquiti bullet (Process varies by OS)
|
||||
* IP Address: 192.168.1.x
|
||||
* This can be just about anything not already in use. I recommend something 30-39
|
||||
* Net Mask: 255.255.255.0
|
||||
* Gateway: 192.168.1.0
|
||||
## Software Prerequisites
|
||||
|
||||
## Launching with ANCHOR
|
||||
You need either [ROS2 Humble](https://docs.ros.org/en/humble/index.html)
|
||||
with [rosdep](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html#rosdep-installation)
|
||||
or [Nix](https://nixos.org/download/#nix-install-linux) installed. We recommend
|
||||
using Nix.
|
||||
|
||||
ANCHOR (Active Node Controller Hub and Operational Relay)
|
||||
Allows for launching all nodes on the rover simulataneously. Additionally, all controls will run through the core's NUC and MCU.
|
||||
<br>
|
||||
1. SSH to core
|
||||
* Core1: `ssh clucky@192.168.1.69`
|
||||
* Core2: `ssh clucky@192.168.1.70`
|
||||
* Password: \<can be found in the rover-Docs repo under documentation>
|
||||
2. Navigate to rover-ros2 workspace
|
||||
* `cd rover-ros2`
|
||||
3. Source the workspace
|
||||
* `source install/setup.bash`
|
||||
4. Launch ANCHOR
|
||||
* `ros2 launch rover_launch.py mode:=anchor`
|
||||
### Nix
|
||||
|
||||
## Launching as Standalone
|
||||
With Nix, all you have to do is enter the development shell:
|
||||
|
||||
For use when running independent modules through their respective computers (pi/NUC) without ANCHOR.
|
||||
```bash
|
||||
$ cd path/to/rover-ros2
|
||||
$ nix develop
|
||||
```
|
||||
|
||||
1. SSH to the the module's computer
|
||||
* Core1: `ssh clucky@192.168.1.69`
|
||||
* Core2: `ssh clucky@192.168.1.70`
|
||||
* Arm: `ssh arm@192.168.1.70`
|
||||
* Bio: \<TBD>
|
||||
* Password: \<can be found in the rover-Docs repo under documentation>
|
||||
2. Run the main node (this sends commands to the MCU)
|
||||
* Navigate to the rover-ros2 workspace (location may vary)
|
||||
* `cd rover-ros2`
|
||||
* Source the workspace
|
||||
* `source install/setup.bash`
|
||||
* Start the node
|
||||
* ARM: `ros2 launch rover_launch.py mode:=arm`
|
||||
* CORE: `ros2 launch rover_launch.py mode:=core`
|
||||
* BIO: `ros2 launch rover_launch.py mode:=bio`
|
||||
### ROS2 Humble + rosdep
|
||||
|
||||
## Running Headless
|
||||
With ROS2 Humble, start by using rosdep to install dependencies:
|
||||
|
||||
Headless control nodes (for ARM and CORE) allow running of the module on-rover without the operator having ROS2 installed on their machine. You will need a laptop to connect to the pi/NUC in order to launch headless but it can be disconnected after the nodes are spun up.
|
||||
<br>
|
||||
1. SSH to the the module's computer
|
||||
* Core1: `ssh clucky@192.168.1.69`
|
||||
* Core2: `ssh clucky@192.168.1.70`
|
||||
* Arm: `ssh arm@192.168.1.70`
|
||||
* Password: \<can be found in the rover-Docs repo under documentation>
|
||||
2. Run the headless node
|
||||
* You must have ANCHOR or the module's Standalone node running
|
||||
* Open a new terminal (SSH'd to the module)
|
||||
* Navigate to rover-ros2 workspace
|
||||
* `cd rover-ros2`
|
||||
* Source the workspace
|
||||
* `source install/setup.bash`
|
||||
* Run the node (ensure controller is connected and on x-input mode)
|
||||
* CORE: `ros2 run core_pkg headless`
|
||||
* ARM: `ros2 run arm_pkg headless`
|
||||
```bash
|
||||
# Setup rosdep
|
||||
$ sudo rosdep init # only run if you haven't already
|
||||
$ rosdep update
|
||||
# Install dependencies
|
||||
$ cd path/to/rover-ros2
|
||||
$ rosdep install --from-paths src -y --ignore-src
|
||||
```
|
||||
|
||||
## Connecting the GuliKit Controller
|
||||
## Running
|
||||
|
||||
Connecting the GuliKit Controller (Recommended)
|
||||
```bash
|
||||
$ colcon build
|
||||
$ source install/setup.bash
|
||||
# main launch files:
|
||||
$ ros2 launch anchor_pkg rover.launch.py # Must be run on a computer connected to a MCU on the rover.
|
||||
$ ros2 run headless_pkg headless_full # Optionally run in a separate shell on the same or different computer.
|
||||
```
|
||||
|
||||
* Connect controller to pc with USB-C
|
||||
### Testing Serial
|
||||
|
||||
You can fake the presence of a Serial device (i.e., MCU) by using the following command:
|
||||
|
||||
```bash
|
||||
$ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT
|
||||
```
|
||||
|
||||
When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
|
||||
|
||||
```bash
|
||||
$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
|
||||
```
|
||||
|
||||
### Connecting the GuliKit Controller
|
||||
|
||||
These instructions apply to the black XBox-style GuliKit controller, primarily used for controlling Arm through Basestation.
|
||||
|
||||
* Connect the controller to your PC with a USB-C cable
|
||||
* Select the "X-Input" control mode (Windows logo) on the controller.
|
||||
* Hold the button next to the symbols (windows, android, switch, etc...)
|
||||
* You'll need to release the button and press down again to cycle to the next mode
|
||||
* Hold the button next to the symbols (windows, android, switch, etc...)
|
||||
* You'll need to release the button and press down again to cycle to the next mode
|
||||
|
||||
## Common Problems/Troubleshooting
|
||||
|
||||
**Q**: When I try to launch the nodes, I receive a `package '' not found` error.
|
||||
|
||||
A: Make sure you have sourced the workspace in the current shell:
|
||||
|
||||
```bash
|
||||
$ source install/setup.bash # or setup.zsh if using ZSH
|
||||
```
|
||||
|
||||
**Q**: When I try to launch the nodes, I receive several `FileNotFoundError: [Errno 2]` errors.
|
||||
|
||||
A: Sometimes the install files get messed up by running `colcon build` in different shells or updating packages. Try running the following commands to clean up your local build files:
|
||||
|
||||
```bash
|
||||
$ rm -rf build/ install/
|
||||
$ colcon build
|
||||
```
|
||||
|
||||
**Q**: When I run `colcon build` after the above suggestion, I receive several of the following errors:
|
||||
|
||||
```bash
|
||||
[0.557s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '' in the environment variable AMENT_PREFIX_PATH doesn't exist
|
||||
```
|
||||
|
||||
A: Don't worry about it. If you had the workspace sourced, ROS2 will complain about the workspace install files not existing anymore after you deleted them. They will be re-created by `colcon build`, after which you can run `source install/setup.bash` to source the new install files.
|
||||
|
||||
**Q**: When I try to launch Anchor, I receive the following errors:
|
||||
|
||||
```bash
|
||||
[anchor-5] [INFO] [1762239452.937881841] [anchor]: Unable to find MCU...
|
||||
...
|
||||
[ERROR] [anchor-5]: process has died [pid 101820, exit code 1, cmd '.../rover-ros2/install/anchor_pkg/lib/anchor_pkg/anchor --ros-args -r __node:=anchor --params-file /tmp/launch_params_nmv6tpw4'].
|
||||
[INFO] [launch]: process[anchor-5] was required: shutting down launched system
|
||||
[INFO] [bio-4]: sending signal 'SIGINT' to process[bio-4]
|
||||
[INFO] [ptz-3]: sending signal 'SIGINT' to process[ptz-3]
|
||||
[INFO] [core-2]: sending signal 'SIGINT' to process[core-2]
|
||||
[INFO] [arm-1]: sending signal 'SIGINT' to process[arm-1]
|
||||
...
|
||||
```
|
||||
|
||||
A: To find a microcontroller to talk to, Anchor sends a ping to every Serial port on your computer. If it does not receive a 'pong' in less than one second, then it aborts. There are a few possible fixes:
|
||||
|
||||
- Keep trying to run it until it works
|
||||
- Run `lsusb` to see if the microcontroller is detected by your computer.
|
||||
- Run `ls /dev/tty*0` to see if there is a valid Serial port enumerated for the microcontroller.
|
||||
- Check if you are in the `dialout` group (or whatever group shows up by running `ls -l /dev/tty*`).
|
||||
|
||||
## Packages
|
||||
|
||||
- [anchor\_pkg](./src/anchor_pkg) - Handles Serial communication between the various other packages here and the microcontroller.
|
||||
- [arm\_pkg](./src/arm_pkg) - Relays controls and sensor data for the arm (socket and digit) between anchor and basestation/headless.
|
||||
- [astra\_descriptions](./src/astra_descriptions) - Submodule with URDF-related packages.
|
||||
- [bio\_pkg](./src/bio_pkg) - Like arm_pkg, but for CITADEL and FAERIE
|
||||
- [core\_pkg](./src/core_pkg) - Like arm_pkg, but for Core
|
||||
- [headless\_pkg](./src/headless_pkg) - Simple, non-graphical controller node to work in place of basestation when controlling the rover by itself. This is autostarted with anchor to allow for setup-less control of the rover.
|
||||
- [latency\_tester](./src/latency_tester) - A temporary node to test comms latency over ROS2, Serial, and CAN.
|
||||
- [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.
|
||||
|
||||
## Maintainers
|
||||
|
||||
| Name | Email | Discord |
|
||||
| ---- | ----- | ------- |
|
||||
| David Sharpe | <ds0196@uah.edu> | `@ddavdd` |
|
||||
| Riley McLain | <rjm0037@uah.edu> | `@ryleu` |
|
||||
|
||||
@@ -1,4 +1,7 @@
|
||||
#!/bin/bash
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
|
||||
|
||||
# Wait for a network interface to be up (not necessarily online)
|
||||
while ! ip link show | grep -q "state UP"; do
|
||||
@@ -15,10 +18,7 @@ echo "[INFO] Starting ROS node..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source your workspace setup script
|
||||
source /home/clucky/rover-ros2/install/setup.bash
|
||||
|
||||
# CD to directory
|
||||
cd /home/clucky/rover-ros2/
|
||||
source $SCRIPT_DIR/../install/setup.bash
|
||||
|
||||
# Launch the ROS 2 node with the desired mode
|
||||
ros2 launch anchor_pkg rover.launch.py mode:=anchor
|
||||
|
||||
@@ -1,24 +0,0 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Wait for a network interface to be up (not necessarily online)
|
||||
while ! ip link show | grep -q "state UP"; do
|
||||
echo "[INFO] Waiting for active network interface..."
|
||||
sleep 1
|
||||
done
|
||||
|
||||
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
|
||||
source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source your workspace setup script
|
||||
source /home/clucky/rover-ros2/install/setup.bash
|
||||
|
||||
# CD to directory
|
||||
cd /home/clucky/rover-ros2/
|
||||
|
||||
# Launch the ROS 2 node
|
||||
ros2 run core_pkg headless
|
||||
@@ -1,4 +1,7 @@
|
||||
#!/bin/bash
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
|
||||
|
||||
# Wait for a network interface to be up (not necessarily online)
|
||||
while ! ip link show | grep -q "state UP"; do
|
||||
@@ -15,10 +18,7 @@ echo "[INFO] Starting ROS node..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source your workspace setup script
|
||||
source /home/clucky/rover-ros2/install/setup.bash
|
||||
|
||||
# CD to directory
|
||||
cd /home/clucky/rover-ros2/
|
||||
source $SCRIPT_DIR/../install/setup.bash
|
||||
|
||||
# Launch the ROS 2 node
|
||||
ros2 run headless_pkg headless_full
|
||||
|
||||
@@ -1,8 +1,12 @@
|
||||
#!/bin/bash
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
ANCHOR_WS="/home/clucky/rover-ros2"
|
||||
AUTONOMY_WS="/home/clucky/rover-Autonomy"
|
||||
BAG_LOCATION="/home/clucky/bags/autostart"
|
||||
SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
|
||||
|
||||
[[ -z "$ANCHOR_WS" ]] && ANCHOR_WS="$SCRIPT_DIR/.."
|
||||
[[ -z "$AUTONOMY_WS" ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
|
||||
BAG_LOCATION="$HOME/bags/autostart"
|
||||
[[ ! -d "$BAG_LOCATION" ]] && mkdir -p "$BAG_LOCATION"
|
||||
|
||||
# Wait for a network interface to be up (not necessarily online)
|
||||
while ! ip link show | grep -q "state UP"; do
|
||||
@@ -15,7 +19,7 @@ echo "[INFO] Network interface is up!"
|
||||
|
||||
source /opt/ros/humble/setup.bash
|
||||
source $ANCHOR_WS/install/setup.bash
|
||||
[ -f $AUTONOMY_WS/install/setup.bash ] && source $AUTONOMY_WS/install/setup.bash
|
||||
[[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash
|
||||
|
||||
cd $BAG_LOCATION
|
||||
|
||||
|
||||
18
docker-compose.yml
Normal file
18
docker-compose.yml
Normal file
@@ -0,0 +1,18 @@
|
||||
|
||||
services:
|
||||
ros2:
|
||||
image: osrf/ros:jazzy-desktop-full
|
||||
container_name: ros2_jazzy_gui
|
||||
network_mode: host
|
||||
environment:
|
||||
- DISPLAY=${DISPLAY}
|
||||
- QT_X11_NO_MITSHM=1
|
||||
- XAUTHORITY=/tmp/.docker.xauth
|
||||
- NVIDIA_VISIBLE_DEVICES=all
|
||||
- NVIDIA_DRIVER_CAPABILITIES=all
|
||||
volumes:
|
||||
- /tmp/.X11-unix:/tmp/.X11-unix:rw
|
||||
- ./ros_ws:/ros_ws
|
||||
stdin_open: true
|
||||
tty: true
|
||||
privileged: true
|
||||
6
flake.lock
generated
6
flake.lock
generated
@@ -24,11 +24,11 @@
|
||||
"nixpkgs": "nixpkgs"
|
||||
},
|
||||
"locked": {
|
||||
"lastModified": 1758094726,
|
||||
"narHash": "sha256-agLnClczRtYY+kQFh5dv4wGNhtFNKK7KFOmypDhsWCs=",
|
||||
"lastModified": 1761810010,
|
||||
"narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
|
||||
"owner": "lopsided98",
|
||||
"repo": "nix-ros-overlay",
|
||||
"rev": "9d0557032aadb65df065b1972a632572b57234b5",
|
||||
"rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
|
||||
"type": "github"
|
||||
},
|
||||
"original": {
|
||||
|
||||
17
flake.nix
17
flake.nix
@@ -53,6 +53,23 @@
|
||||
robot-state-publisher
|
||||
ros2-control
|
||||
controller-manager
|
||||
control-msgs
|
||||
control-toolbox
|
||||
moveit-core
|
||||
moveit-common
|
||||
moveit-msgs
|
||||
moveit-ros-planning
|
||||
moveit-ros-planning-interface
|
||||
moveit-configs-utils
|
||||
moveit-ros-move-group
|
||||
moveit-servo
|
||||
moveit-simple-controller-manager
|
||||
topic-based-ros2-control
|
||||
pilz-industrial-motion-planner
|
||||
pick-ik
|
||||
ompl
|
||||
chomp-motion-planner
|
||||
joy
|
||||
# ros2-controllers nixpkg does not build :(
|
||||
];
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@ import threading
|
||||
import glob
|
||||
|
||||
from std_msgs.msg import String, Header
|
||||
from ros2_interfaces_pkg.msg import VicCAN
|
||||
from astra_msgs.msg import VicCAN
|
||||
|
||||
serial_pub = None
|
||||
thread = None
|
||||
@@ -38,10 +38,12 @@ Subscribers:
|
||||
* /anchor/to_vic/relay_string
|
||||
- Publish raw strings to this topic to send directly to the MCU for debugging
|
||||
"""
|
||||
|
||||
|
||||
class SerialRelay(Node):
|
||||
def __init__(self):
|
||||
# Initalize node with name
|
||||
super().__init__("anchor_node")#previously 'serial_publisher'
|
||||
super().__init__("anchor_node") # previously 'serial_publisher'
|
||||
|
||||
# Loop through all serial devices on the computer to check for the MCU
|
||||
self.port = None
|
||||
@@ -55,7 +57,7 @@ class SerialRelay(Node):
|
||||
try:
|
||||
# connect and send a ping command
|
||||
ser = serial.Serial(port, 115200, timeout=1)
|
||||
#(f"Checking port {port}...")
|
||||
# (f"Checking port {port}...")
|
||||
ser.write(b"ping\n")
|
||||
response = ser.read_until(bytes("\n", "utf8"))
|
||||
|
||||
@@ -78,26 +80,40 @@ class SerialRelay(Node):
|
||||
atexit.register(self.cleanup)
|
||||
|
||||
# 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)
|
||||
|
||||
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)
|
||||
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
|
||||
)
|
||||
|
||||
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
|
||||
)
|
||||
|
||||
# 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)
|
||||
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)
|
||||
|
||||
self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
|
||||
self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
|
||||
|
||||
# Create a subscriber
|
||||
self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
|
||||
|
||||
self.relay_sub = self.create_subscription(
|
||||
String, "/anchor/relay", self.on_relay_tovic_string, 10
|
||||
)
|
||||
|
||||
def run(self):
|
||||
# This thread makes all the update processes run in the background
|
||||
@@ -112,22 +128,26 @@ class SerialRelay(Node):
|
||||
sys.exit(0)
|
||||
|
||||
def read_MCU(self):
|
||||
""" Check the USB serial port for new data from the MCU, and publish string to appropriate topics """
|
||||
"""Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
|
||||
try:
|
||||
output = str(self.ser.readline(), "utf8")
|
||||
|
||||
if output:
|
||||
self.relay_fromvic(output)
|
||||
# All output over debug temporarily
|
||||
#self.get_logger().info(f"[MCU] {output}")
|
||||
# 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
|
||||
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
|
||||
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
|
||||
@@ -152,15 +172,13 @@ class SerialRelay(Node):
|
||||
# self.ser.close()
|
||||
# exit(1)
|
||||
|
||||
|
||||
def on_mock_fromvic(self, msg: String):
|
||||
""" For testing without an actual MCU, publish strings here as if they came from an MCU """
|
||||
"""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 """
|
||||
"""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
|
||||
@@ -169,7 +187,7 @@ class SerialRelay(Node):
|
||||
self.ser.write(bytes(output, "utf8"))
|
||||
|
||||
def relay_fromvic(self, msg: str):
|
||||
""" Relay a string message from the MCU to the appropriate VicCAN topic """
|
||||
"""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":
|
||||
@@ -181,11 +199,13 @@ class SerialRelay(Node):
|
||||
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)})"
|
||||
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:
|
||||
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:
|
||||
@@ -198,7 +218,9 @@ class SerialRelay(Node):
|
||||
break
|
||||
|
||||
if malformed:
|
||||
self.get_logger().warning(f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}")
|
||||
self.get_logger().warning(
|
||||
f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
|
||||
)
|
||||
return
|
||||
|
||||
# Have valid VicCAN message
|
||||
@@ -208,7 +230,9 @@ class SerialRelay(Node):
|
||||
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")
|
||||
output.header = Header(
|
||||
stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
|
||||
)
|
||||
|
||||
# self.get_logger().info(f"Relaying from MCU: {output}")
|
||||
if output.mcu_name == "core":
|
||||
@@ -218,11 +242,10 @@ class SerialRelay(Node):
|
||||
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 """
|
||||
"""Relay a raw string message to the MCU for debugging"""
|
||||
message = msg.data
|
||||
#self.get_logger().info(f"Sending command to MCU: {msg}")
|
||||
# self.get_logger().info(f"Sending command to MCU: {msg}")
|
||||
self.ser.write(bytes(message, "utf8"))
|
||||
|
||||
@staticmethod
|
||||
@@ -234,6 +257,7 @@ class SerialRelay(Node):
|
||||
if self.ser.is_open:
|
||||
self.ser.close()
|
||||
|
||||
|
||||
def myexcepthook(type, value, tb):
|
||||
print("Uncaught exception:", type, value)
|
||||
if serial_pub:
|
||||
@@ -249,7 +273,10 @@ def main(args=None):
|
||||
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
|
||||
|
||||
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()
|
||||
|
||||
@@ -2,115 +2,121 @@
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.substitutions import (
|
||||
LaunchConfiguration,
|
||||
ThisLaunchFileDir,
|
||||
PathJoinSubstitution,
|
||||
)
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
|
||||
#Prevent making __pycache__ directories
|
||||
# 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)
|
||||
mode = LaunchConfiguration("mode").perform(context)
|
||||
nodes = []
|
||||
|
||||
if mode == 'anchor':
|
||||
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()
|
||||
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()
|
||||
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'
|
||||
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()
|
||||
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()
|
||||
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']:
|
||||
elif mode in ["arm", "core", "bio", "ptz"]:
|
||||
# Only launch the node corresponding to the provided mode.
|
||||
if mode == 'arm':
|
||||
if mode == "arm":
|
||||
nodes.append(
|
||||
Node(
|
||||
package='arm_pkg',
|
||||
executable='arm',
|
||||
name='arm',
|
||||
output='both',
|
||||
parameters=[{'launch_mode': mode}],
|
||||
on_exit=Shutdown()
|
||||
package="arm_pkg",
|
||||
executable="arm",
|
||||
name="arm",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
elif mode == 'core':
|
||||
elif mode == "core":
|
||||
nodes.append(
|
||||
Node(
|
||||
package='core_pkg',
|
||||
executable='core',
|
||||
name='core',
|
||||
output='both',
|
||||
parameters=[{'launch_mode': mode}],
|
||||
on_exit=Shutdown()
|
||||
package="core_pkg",
|
||||
executable="core",
|
||||
name="core",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
elif mode == 'bio':
|
||||
elif mode == "bio":
|
||||
nodes.append(
|
||||
Node(
|
||||
package='bio_pkg',
|
||||
executable='bio',
|
||||
name='bio',
|
||||
output='both',
|
||||
parameters=[{'launch_mode': mode}],
|
||||
on_exit=Shutdown()
|
||||
package="bio_pkg",
|
||||
executable="bio",
|
||||
name="bio",
|
||||
output="both",
|
||||
parameters=[{"launch_mode": mode}],
|
||||
on_exit=Shutdown(),
|
||||
)
|
||||
)
|
||||
elif mode == 'ptz':
|
||||
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
|
||||
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:
|
||||
@@ -119,14 +125,12 @@ def launch_setup(context, *args, **kwargs):
|
||||
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
declare_arg = DeclareLaunchArgument(
|
||||
'mode',
|
||||
default_value='anchor',
|
||||
description='Launch mode: arm, core, bio, anchor, or ptz'
|
||||
"mode",
|
||||
default_value="anchor",
|
||||
description="Launch mode: arm, core, bio, anchor, or ptz",
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
declare_arg,
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
|
||||
|
||||
@@ -2,27 +2,24 @@ from setuptools import find_packages, setup
|
||||
from os import path
|
||||
from glob import glob
|
||||
|
||||
package_name = 'anchor_pkg'
|
||||
package_name = "anchor_pkg"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
version="0.0.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
(path.join("share", package_name), ['package.xml']),
|
||||
(path.join("share", package_name, "launch"), glob("launch/*"))
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
(path.join("share", package_name), ["package.xml"]),
|
||||
(path.join("share", package_name, "launch"), glob("launch/*")),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer='tristan',
|
||||
maintainer_email='tristanmcginnis26@gmail.com',
|
||||
description='Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN',
|
||||
license='All Rights Reserved',
|
||||
maintainer="tristan",
|
||||
maintainer_email="tristanmcginnis26@gmail.com",
|
||||
description="Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN",
|
||||
license="All Rights Reserved",
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
"anchor = anchor_pkg.anchor_node:main"
|
||||
],
|
||||
"console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -12,14 +12,17 @@ import glob
|
||||
import os
|
||||
|
||||
from std_msgs.msg import String
|
||||
from ros2_interfaces_pkg.msg import ControllerState
|
||||
from ros2_interfaces_pkg.msg import ArmManual
|
||||
from ros2_interfaces_pkg.msg import ArmIK
|
||||
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_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
|
||||
@@ -30,20 +33,18 @@ class Headless(Node):
|
||||
|
||||
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)
|
||||
# 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.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
|
||||
|
||||
@@ -66,38 +67,36 @@ class Headless(Node):
|
||||
# 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()}')
|
||||
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
|
||||
# Check the pico for updates
|
||||
|
||||
#self.read_feedback()
|
||||
if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected
|
||||
# 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_controls() #depricated, kept for reference temporarily
|
||||
self.send_manual()
|
||||
#self.read_feedback()
|
||||
# self.read_feedback()
|
||||
self.gamepad = pygame.joystick.Joystick(0)
|
||||
self.gamepad.init() #re-initialized gamepad
|
||||
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:
|
||||
@@ -105,21 +104,20 @@ class Headless(Node):
|
||||
exit()
|
||||
input = ArmManual()
|
||||
|
||||
|
||||
# Triggers for gripper control
|
||||
if self.gamepad.get_axis(2) > 0:#left trigger
|
||||
if self.gamepad.get_axis(2) > 0: # left trigger
|
||||
input.gripper = -1
|
||||
elif self.gamepad.get_axis(5) > 0:#right trigger
|
||||
elif self.gamepad.get_axis(5) > 0: # right trigger
|
||||
input.gripper = 1
|
||||
|
||||
# Toggle Laser
|
||||
if self.gamepad.get_button(7):#Start
|
||||
if self.gamepad.get_button(7): # Start
|
||||
self.laser_status = 1
|
||||
elif self.gamepad.get_button(6):#Back
|
||||
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
|
||||
if self.gamepad.get_button(5): # right bumper, control effector
|
||||
|
||||
# Left stick X-axis for effector yaw
|
||||
if self.gamepad.get_axis(0) > 0:
|
||||
@@ -141,38 +139,38 @@ class Headless(Node):
|
||||
elif dpad_input[0] == -1:
|
||||
input.axis0 = -1
|
||||
|
||||
if self.gamepad.get_axis(0) > .15 or self.gamepad.get_axis(0) < -.15:
|
||||
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) > .15 or self.gamepad.get_axis(1) < -.15:
|
||||
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) > .15 or self.gamepad.get_axis(4) < -.15:
|
||||
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
|
||||
# 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.get_logger().info(
|
||||
f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n"
|
||||
)
|
||||
self.manual_pub.publish(input)
|
||||
else:
|
||||
pass
|
||||
@@ -241,14 +239,12 @@ class Headless(Node):
|
||||
# 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:
|
||||
@@ -258,7 +254,6 @@ class Headless(Node):
|
||||
# else:
|
||||
# input.d_left = False
|
||||
|
||||
|
||||
# if pygame.joystick.get_count() != 0:
|
||||
|
||||
# self.get_logger().info(f"[Ctrl] Updated Controller State\n")
|
||||
@@ -268,9 +263,6 @@ class Headless(Node):
|
||||
# pass
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
@@ -279,9 +271,9 @@ def main(args=None):
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
|
||||
#tb_bs = BaseStation()
|
||||
#node.run()
|
||||
# tb_bs = BaseStation()
|
||||
# node.run()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy import qos
|
||||
import serial
|
||||
import sys
|
||||
import threading
|
||||
@@ -9,23 +8,11 @@ import time
|
||||
import atexit
|
||||
import signal
|
||||
from std_msgs.msg import String
|
||||
from ros2_interfaces_pkg.msg import ArmManual
|
||||
from ros2_interfaces_pkg.msg import ArmIK
|
||||
from ros2_interfaces_pkg.msg import SocketFeedback
|
||||
from ros2_interfaces_pkg.msg import DigitFeedback
|
||||
|
||||
|
||||
# IK-Related imports
|
||||
import numpy as np
|
||||
import time, math, os
|
||||
from math import sin, cos, pi
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
# from ikpy.chain import Chain
|
||||
# from ikpy.link import OriginLink, URDFLink
|
||||
# #import pygame as pyg
|
||||
# from scipy.spatial.transform import Rotation as R
|
||||
|
||||
from . import astra_arm
|
||||
from astra_msgs.msg import ArmManual
|
||||
from astra_msgs.msg import SocketFeedback
|
||||
from astra_msgs.msg import DigitFeedback
|
||||
from sensor_msgs.msg import JointState
|
||||
import math
|
||||
|
||||
# control_qos = qos.QoSProfile(
|
||||
# history=qos.QoSHistoryPolicy.KEEP_LAST,
|
||||
@@ -48,42 +35,64 @@ class SerialRelay(Node):
|
||||
super().__init__("arm_node")
|
||||
|
||||
# Get launch mode parameter
|
||||
self.declare_parameter('launch_mode', 'arm')
|
||||
self.launch_mode = self.get_parameter('launch_mode').value
|
||||
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}")
|
||||
|
||||
# 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.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)
|
||||
|
||||
# Create subscribers
|
||||
self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10)
|
||||
self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10)
|
||||
self.man_sub = self.create_subscription(
|
||||
ArmManual, "/arm/control/manual", self.send_manual, 10
|
||||
)
|
||||
|
||||
self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10)
|
||||
# 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
|
||||
|
||||
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)
|
||||
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)
|
||||
|
||||
self.arm = astra_arm.Arm('arm12.urdf')
|
||||
self.arm_feedback = SocketFeedback()
|
||||
self.digit_feedback = DigitFeedback()
|
||||
|
||||
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode
|
||||
if self.launch_mode == 'arm':
|
||||
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 i in range(4):
|
||||
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}...")
|
||||
# print(f"Checking port {port}...")
|
||||
ser.write(b"ping\n")
|
||||
response = ser.read_until("\n") # type: ignore
|
||||
|
||||
@@ -98,7 +107,9 @@ class SerialRelay(Node):
|
||||
break
|
||||
|
||||
if self.port is None:
|
||||
self.get_logger().info("Unable to find MCU... please make sure it is connected.")
|
||||
self.get_logger().info(
|
||||
"Unable to find MCU... please make sure it is connected."
|
||||
)
|
||||
time.sleep(1)
|
||||
sys.exit(1)
|
||||
|
||||
@@ -110,11 +121,11 @@ class SerialRelay(Node):
|
||||
thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
|
||||
thread.start()
|
||||
|
||||
#if in arm mode, will need to read from the MCU
|
||||
# if in arm mode, will need to read from the MCU
|
||||
|
||||
try:
|
||||
while rclpy.ok():
|
||||
if self.launch_mode == 'arm':
|
||||
if self.launch_mode == "arm":
|
||||
if self.ser.in_waiting:
|
||||
self.read_mcu()
|
||||
else:
|
||||
@@ -124,13 +135,12 @@ class SerialRelay(Node):
|
||||
finally:
|
||||
self.cleanup()
|
||||
|
||||
|
||||
#Currently will just spit out all values over the /arm/feedback/debug topic as strings
|
||||
# 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}")
|
||||
# self.get_logger().info(f"[MCU] {output}")
|
||||
msg = String()
|
||||
msg.data = output
|
||||
self.debug_pub.publish(msg)
|
||||
@@ -152,79 +162,19 @@ class SerialRelay(Node):
|
||||
self.ser.close()
|
||||
pass
|
||||
|
||||
def send_ik(self, msg):
|
||||
# Convert Vector3 to a NumPy array
|
||||
input_raw = np.array([-msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z]) # Convert input to a NumPy array
|
||||
# decrease input vector by 90%
|
||||
input_raw = input_raw * 0.2
|
||||
def joint_command_callback(self, msg: JointState):
|
||||
# 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]
|
||||
|
||||
if input_raw[0] == 0.0 and input_raw[1] == 0.0 and input_raw[2] == 0.0:
|
||||
self.get_logger().info("No input, stopping arm.")
|
||||
command = "can_relay_tovic,arm,39,0,0,0,0\n"
|
||||
# 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)
|
||||
return
|
||||
|
||||
# Debug output
|
||||
tempMsg = String()
|
||||
tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
|
||||
#self.debug_pub.publish(tempMsg)
|
||||
|
||||
# Target position is current position + input vector
|
||||
current_position = self.arm.get_position_vector()
|
||||
target_position = current_position + input_raw
|
||||
|
||||
|
||||
#Print for IK DEBUG
|
||||
tempMsg = String()
|
||||
# tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles)
|
||||
tempMsg.data = "Current Angles: " + str(math.degrees(self.arm.current_angles[2])) + ", " + str(math.degrees(self.arm.current_angles[4])) + ", " + str(math.degrees(self.arm.current_angles[6]))
|
||||
self.ik_debug.publish(tempMsg)
|
||||
self.get_logger().info(f"[IK] {tempMsg.data}")
|
||||
|
||||
# Debug output for current position
|
||||
#tempMsg.data = "Current Position: " + str(current_position)
|
||||
#self.debug_pub.publish(tempMsg)
|
||||
|
||||
# Debug output for target position
|
||||
#tempMsg.data = "Target Position: " + str(target_position)
|
||||
#self.debug_pub.publish(tempMsg)
|
||||
|
||||
# Perform IK with the target position
|
||||
if self.arm.perform_ik(target_position, self.get_logger()):
|
||||
# Send command to control
|
||||
|
||||
#command = "can_relay_tovic,arm,32," + ",".join(map(str, self.arm.ik_angles[:4])) + "\n"
|
||||
#self.send_cmd(command)
|
||||
self.get_logger().info(f"IK Success: {target_position}")
|
||||
self.get_logger().info(f"IK Angles: [{str(round(math.degrees(self.arm.ik_angles[2]), 2))}, {str(round(math.degrees(self.arm.ik_angles[4]), 2))}, {str(round(math.degrees(self.arm.ik_angles[6]), 2))}]")
|
||||
|
||||
# tempMsg = String()
|
||||
# tempMsg.data = "IK Success: " + str(target_position)
|
||||
# #self.debug_pub.publish(tempMsg)
|
||||
# tempMsg.data = "Sending: " + str(command)
|
||||
#self.debug_pub.publish(tempMsg)
|
||||
|
||||
# Send the IK angles to the MCU
|
||||
command = "can_relay_tovic,arm,32," + f"{math.degrees(self.arm.ik_angles[0])*10},{math.degrees(self.arm.ik_angles[2])*10},{math.degrees(self.arm.ik_angles[4])*10},{math.degrees(self.arm.ik_angles[6])*10}" + "\n"
|
||||
# self.send_cmd(command)
|
||||
|
||||
# Manual control for Wrist/Effector
|
||||
command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
|
||||
|
||||
command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
|
||||
|
||||
command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
|
||||
|
||||
command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
|
||||
|
||||
self.send_cmd(command)
|
||||
else:
|
||||
self.get_logger().info("IK Fail")
|
||||
self.get_logger().info(f"IK Angles: [{str(math.degrees(self.arm.ik_angles[2]))}, {str(math.degrees(self.arm.ik_angles[4]))}, {str(math.degrees(self.arm.ik_angles[6]))}]")
|
||||
# tempMsg = String()
|
||||
# tempMsg.data = "IK Fail"
|
||||
#self.debug_pub.publish(tempMsg)
|
||||
|
||||
|
||||
def send_manual(self, msg: ArmManual):
|
||||
axis0 = msg.axis0
|
||||
@@ -232,42 +182,42 @@ class SerialRelay(Node):
|
||||
axis2 = msg.axis2
|
||||
axis3 = msg.axis3
|
||||
|
||||
#Send controls for arm
|
||||
command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n"
|
||||
command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n"
|
||||
# Send controls for arm
|
||||
command = f"can_relay_tovic,arm,18,{int(msg.brake)}\n"
|
||||
command += f"can_relay_tovic,arm,39,{axis0},{axis1},{axis2},{axis3}\n"
|
||||
|
||||
#Send controls for end effector
|
||||
# Send controls for end effector
|
||||
|
||||
# command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
|
||||
# command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
|
||||
command += "can_relay_tovic,digit,39," + str(msg.effector_yaw) + "," + str(msg.effector_roll) + "\n"
|
||||
command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
|
||||
|
||||
command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
|
||||
# command += f"can_relay_tovic,digit,26,{msg.gripper}\n" # no hardware rn
|
||||
|
||||
command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
|
||||
command += f"can_relay_tovic,digit,28,{msg.laser}\n"
|
||||
|
||||
command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n"
|
||||
command += f"can_relay_tovic,digit,34,{msg.linear_actuator}\n"
|
||||
|
||||
self.send_cmd(command)
|
||||
|
||||
return
|
||||
|
||||
def send_cmd(self, msg: str):
|
||||
if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
|
||||
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
|
||||
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"))
|
||||
|
||||
def anchor_feedback(self, msg: String):
|
||||
output = msg.data
|
||||
if output.startswith("can_relay_fromvic,arm,55"):
|
||||
#pass
|
||||
# pass
|
||||
self.updateAngleFeedback(output)
|
||||
elif output.startswith("can_relay_fromvic,arm,54"):
|
||||
#pass
|
||||
# pass
|
||||
self.updateBusVoltage(output)
|
||||
elif output.startswith("can_relay_fromvic,arm,53"):
|
||||
self.updateMotorFeedback(output)
|
||||
@@ -285,6 +235,12 @@ class SerialRelay(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
|
||||
|
||||
@@ -302,28 +258,21 @@ class SerialRelay(Node):
|
||||
angles_in = parts[3:7]
|
||||
# Convert the angles to floats divide by 10.0
|
||||
angles = [float(angle) / 10.0 for angle in angles_in]
|
||||
# angles[0] = 0.0 #override axis0 to zero
|
||||
#
|
||||
#
|
||||
#THIS NEEDS TO BE REMOVED LATER
|
||||
#PLACEHOLDER FOR WRIST VALUE
|
||||
#
|
||||
#
|
||||
angles.append(0.0)#placeholder for wrist_continuous
|
||||
angles.append(0.0)#placeholder for wrist
|
||||
#
|
||||
#
|
||||
# # Update the arm's current angles
|
||||
self.arm.update_angles(angles)
|
||||
|
||||
self.arm_feedback.axis0_angle = angles[0]
|
||||
self.arm_feedback.axis1_angle = angles[1]
|
||||
self.arm_feedback.axis2_angle = angles[2]
|
||||
self.arm_feedback.axis3_angle = angles[3]
|
||||
# self.get_logger().info(f"Angles: {angles}")
|
||||
# #debug publish angles
|
||||
# tempMsg = String()
|
||||
# tempMsg.data = "Angles: " + str(angles)
|
||||
# #self.debug_pub.publish(tempMsg)
|
||||
|
||||
# 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")
|
||||
|
||||
@@ -364,11 +313,10 @@ class SerialRelay(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]*")
|
||||
# return glob.glob("/dev/tty[A-Za-z]*")
|
||||
|
||||
def cleanup(self):
|
||||
print("Cleaning up...")
|
||||
@@ -378,11 +326,13 @@ class SerialRelay(Node):
|
||||
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
|
||||
@@ -391,7 +341,10 @@ def main(args=None):
|
||||
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
|
||||
|
||||
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,145 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import numpy as np
|
||||
import time, math, os
|
||||
from math import sin, cos, pi
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from ikpy.chain import Chain
|
||||
from ikpy.link import OriginLink, URDFLink
|
||||
#import pygame as pyg
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
from geometry_msgs.msg import Vector3
|
||||
|
||||
|
||||
# Misc
|
||||
degree = pi / 180.0
|
||||
|
||||
|
||||
def convert_angles(angles):
|
||||
# Converts angles to the format used for the urdf (contains some dummy joints)
|
||||
return [0.0, math.radians(angles[0]), math.radians(angles[1]), 0.0, math.radians(angles[2]), 0.0, math.radians(angles[3]), 0.0, math.radians(angles[4]), math.radians(angles[5]), 0.0]
|
||||
|
||||
|
||||
class Arm:
|
||||
def __init__(self, urdf_name):
|
||||
self.ik_tolerance = 1e-1 #Tolerance (in meters) to determine if solution is valid
|
||||
# URDF file path
|
||||
self.urdf = os.path.join(get_package_share_directory('arm_pkg'), 'urdf', urdf_name)
|
||||
# IKpy Chain
|
||||
self.chain = Chain.from_urdf_file(self.urdf)
|
||||
|
||||
|
||||
# Arrays for joint states
|
||||
# Some links in the URDF are static (non-joints), these will remain zero for IK
|
||||
# Indexes: Fixed_base, Ax_0, Ax_1, seg1, Ax_2, seg2, ax_3, seg3, continuous, wrist, Effector
|
||||
self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.current_angles = self.zero_angles
|
||||
self.last_angles = self.zero_angles
|
||||
self.ik_angles = self.zero_angles
|
||||
|
||||
self.current_position: list[float] = []
|
||||
self.target_position = [0.0, 0.0, 0.0]
|
||||
self.target_orientation: list = [] # Effector orientation desired at target position.
|
||||
# Generally orientation for the effector is modified manually by the operator.
|
||||
|
||||
# Might not need, copied over from state_publisher.py in ik_test
|
||||
#self.step = 0.03 # Max movement increment
|
||||
|
||||
|
||||
def perform_ik(self, target_position, logger):
|
||||
self.target_position = target_position
|
||||
# Update the target orientation to the current orientation
|
||||
self.update_orientation()
|
||||
# print(f"[IK FOR] Target Position: {self.target_position}")
|
||||
try:
|
||||
# print(f"[TRY] Current Angles: {self.current_angles}")
|
||||
# print(f"[TRY] Target Position: {self.target_position}")
|
||||
# print(f"[TRY] Target Orientation: {self.target_orientation}")
|
||||
self.ik_angles = self.chain.inverse_kinematics(
|
||||
target_position=self.target_position,
|
||||
target_orientation=self.target_orientation,
|
||||
initial_position=self.current_angles,
|
||||
orientation_mode="all"
|
||||
)
|
||||
# Check if the solution is within the tolerance
|
||||
fk_matrix = self.chain.forward_kinematics(self.ik_angles)
|
||||
|
||||
fk_position = fk_matrix[:3, 3] # type: ignore
|
||||
|
||||
# print(f"[TRY] FK Position for Solution: {fk_position}")
|
||||
|
||||
error = np.linalg.norm(target_position - fk_position)
|
||||
if error > self.ik_tolerance:
|
||||
logger.info(f"No VALID IK Solution within tolerance. Error: {error}")
|
||||
return False
|
||||
else:
|
||||
logger.info(f"IK Solution Found. Error: {error}")
|
||||
return True
|
||||
except Exception as e:
|
||||
logger.info(f"IK failed for exception: {e}")
|
||||
return False
|
||||
|
||||
# # Given the FK_Matix for the arm's current pose, update the orientation array
|
||||
# def update_orientation(self, fk_matrix):
|
||||
# self.target_orientation = fk_matrix[:3, :3]
|
||||
# return
|
||||
|
||||
# def update_joints(self, ax_0, ax_1, ax_2, ax_3, wrist):
|
||||
# self.current_angles = [0.0, 0.0, ax_0, ax_1, ax_2, ax_3, wrist, 0.0]
|
||||
# return
|
||||
|
||||
# Get current orientation of the end effector and update target_orientation
|
||||
def update_orientation(self):
|
||||
|
||||
# FK matrix for arm's current pose
|
||||
fk_matrix = self.chain.forward_kinematics(self.current_angles)
|
||||
|
||||
# Update target_orientation to the effector's current orientation
|
||||
self.target_orientation = fk_matrix[:3, :3] # type: ignore
|
||||
|
||||
# Update current angles to those provided
|
||||
# Resetting last_angles to the new angles
|
||||
#
|
||||
# Use: First call, or when angles are changed manually.
|
||||
def reset_angles(self, angles):
|
||||
# Update angles to the new angles
|
||||
self.current_angles = convert_angles(angles)
|
||||
self.last_angles = self.current_angles
|
||||
|
||||
# Update current angles to those provided
|
||||
# Maintain previous angles in last_angles
|
||||
#
|
||||
# Use: Repeated calls during IK operation
|
||||
def update_angles(self, angles):
|
||||
# Update angles to the new angles
|
||||
self.last_angles = self.current_angles
|
||||
self.current_angles = convert_angles(angles)
|
||||
|
||||
# Get current X,Y,Z position of end effector
|
||||
def get_position(self):
|
||||
# FK matrix for arm's current pose
|
||||
fk_matrix = self.chain.forward_kinematics(self.current_angles)
|
||||
|
||||
# Get the position of the end effector from the FK matrix
|
||||
position = fk_matrix[:3, 3] # type: ignore
|
||||
|
||||
return position
|
||||
|
||||
# Get current X,Y,Z position of end effector
|
||||
def get_position_vector(self):
|
||||
# FK matrix for arm's current pose
|
||||
fk_matrix = self.chain.forward_kinematics(self.current_angles)
|
||||
|
||||
# Get the position of the end effector from the FK matrix
|
||||
position = fk_matrix[:3, 3] # type: ignore
|
||||
|
||||
# Return position as a NumPy array
|
||||
return np.array(position)
|
||||
|
||||
|
||||
def update_position(self):
|
||||
# FK matrix for arm's current pose
|
||||
fk_matrix = self.chain.forward_kinematics(self.current_angles)
|
||||
|
||||
# Get the position of the end effector from the FK matrix and update current pos
|
||||
self.current_position = fk_matrix[:3, 3] # type: ignore
|
||||
@@ -10,9 +10,7 @@
|
||||
<depend>rclpy</depend>
|
||||
<depend>common_interfaces</depend>
|
||||
<depend>python3-numpy</depend>
|
||||
<depend>ros2_interfaces_pkg</depend>
|
||||
<!-- TODO: remove after refactored out -->
|
||||
<exec_depend>python3-ikpy-pip</exec_depend>
|
||||
<depend>astra_msgs</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
|
||||
@@ -2,3 +2,5 @@
|
||||
script_dir=$base/lib/arm_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/arm_pkg
|
||||
[build_scripts]
|
||||
executable= /usr/bin/env python3
|
||||
|
||||
@@ -2,29 +2,26 @@ from setuptools import find_packages, setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = 'arm_pkg'
|
||||
package_name = "arm_pkg"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
version="1.0.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), glob('launch/*')),
|
||||
(os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer='tristan',
|
||||
maintainer_email='tristanmcginnis26@gmail.com',
|
||||
description='TODO: Package description',
|
||||
license='All Rights Reserved',
|
||||
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',
|
||||
'headless = arm_pkg.arm_headless:main'
|
||||
"console_scripts": [
|
||||
"arm = arm_pkg.arm_node:main",
|
||||
"headless = arm_pkg.arm_headless:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -1,239 +0,0 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from omni_description/robots/omnipointer_arm_only.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="omnipointer" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<!--MX64trntbl + MX106: ? + 0.153 -->
|
||||
<!-- singleaxismount + 2.5girder + singleaxismount + base + MX106: 0.007370876 + 0.0226796 + 0.007370876 + ? + 0.153 -->
|
||||
<!-- frame106 + singleaxismount + adjustgirder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.110563 + 0.00737088 + ? + 0.077 -->
|
||||
<!-- frame28 + singleaxismount + 5girder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.0368544 + 0.00737088 + ? + 0.077 -->
|
||||
<!-- frame28 + singleaxismount + 2.5girder + singleaxismount + tip: 0.0907185 + 0.00737088 + 0.0226796 + 0.00737088 + ? -->
|
||||
<material name="omni/Blue">
|
||||
<color rgba="0 0 0.8 1"/>
|
||||
</material>
|
||||
<material name="omni/Red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
<material name="omni/Green">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
<material name="omni/Yellow">
|
||||
<color rgba="1 1 0 1"/>
|
||||
</material>
|
||||
<material name="omni/LightGrey">
|
||||
<color rgba="0.6 0.6 0.6 1"/>
|
||||
</material>
|
||||
<material name="omni/DarkGrey">
|
||||
<color rgba="0.4 0.4 0.4 1"/>
|
||||
</material>
|
||||
<!-- Now we can start using the macros xacro:included above to define the actual omnipointer -->
|
||||
<!-- The first use of a macro. This one was defined in youbot_base/base.urdf.xacro above.
|
||||
A macro like this will expand to a set of link and joint definitions, and to additional
|
||||
Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
|
||||
that equals "base", and uses it to generate names for its component links and joints
|
||||
(e.g., base_link). The xacro:included origin block is also an argument to the macro. By convention,
|
||||
the origin block defines where the component is w.r.t its parent (in this case the parent
|
||||
is the world frame). For more, see http://www.ros.org/wiki/xacro -->
|
||||
<!-- foot for arm-->
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="10.0"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- joint between base_link and arm_0_link -->
|
||||
<joint name="arm_joint_0" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="arm_link_0"/>
|
||||
</joint>
|
||||
<link name="arm_link_0">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.02725"/>
|
||||
<geometry>
|
||||
<box size="0.1143 0.1143 0.0545"/>
|
||||
</geometry>
|
||||
<material name="omni/LightGrey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.02725"/>
|
||||
<geometry>
|
||||
<box size="0.1143 0.1143 0.0545"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<!-- CENTER OF MASS -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0.02725"/>
|
||||
<mass value="0.2"/>
|
||||
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
|
||||
<inertia ixx="0.000267245666667" ixy="0" ixz="0" iyy="0.000267245666667" iyz="0" izz="0.000435483"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="arm_joint_1" type="revolute">
|
||||
<parent link="arm_link_0"/>
|
||||
<child link="arm_link_1"/>
|
||||
<dynamics damping="3.0" friction="0.3"/>
|
||||
<limit effort="30.0" lower="-3.1415926535" upper="3.1415926535" velocity="5.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0545"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="arm_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.075"/>
|
||||
<geometry>
|
||||
<box size="0.0402 0.05 0.15"/>
|
||||
</geometry>
|
||||
<material name="omni/Blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.075"/>
|
||||
<geometry>
|
||||
<box size="0.0402 0.05 0.15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<!-- CENTER OF MASS -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0.075"/>
|
||||
<mass value="0.190421352"/>
|
||||
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
|
||||
<inertia ixx="0.000279744834534" ixy="0" ixz="0" iyy="0.000265717763008" iyz="0" izz="6.53151584738e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="arm_joint_2" type="revolute">
|
||||
<parent link="arm_link_1"/>
|
||||
<child link="arm_link_2"/>
|
||||
<dynamics damping="3.0" friction="0.3"/>
|
||||
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.15"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
<link name="arm_link_2">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.2355"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.471"/>
|
||||
</geometry>
|
||||
<material name="omni/Red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.2355"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.471"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<!-- CENTER OF MASS -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0.2355"/>
|
||||
<mass value="0.29302326"/>
|
||||
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
|
||||
<inertia ixx="0.00251484771035" ixy="0" ixz="0" iyy="0.00248474836108" iyz="0" izz="9.19936757328e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="arm_joint_3" type="revolute">
|
||||
<parent link="arm_link_2"/>
|
||||
<child link="arm_link_3"/>
|
||||
<dynamics damping="3.0" friction="0.3"/>
|
||||
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.471"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
<link name="arm_link_3">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1885"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.377"/>
|
||||
</geometry>
|
||||
<material name="omni/Yellow"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1885"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.377"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<!-- CENTER OF MASS -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1885"/>
|
||||
<mass value="0.21931466"/>
|
||||
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
|
||||
<inertia ixx="0.000791433503053" ixy="0" ixz="0" iyy="0.000768905501178" iyz="0" izz="6.88531064581e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="arm_joint_4" type="revolute">
|
||||
<parent link="arm_link_3"/>
|
||||
<child link="arm_link_4"/>
|
||||
<dynamics damping="3.0" friction="0.3"/>
|
||||
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.377"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
<link name="arm_link_4">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.066"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.132"/>
|
||||
</geometry>
|
||||
<material name="omni/Green"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.066"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.132"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<!-- CENTER OF MASS -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0.066"/>
|
||||
<mass value="0.15813986"/>
|
||||
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
|
||||
<inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="arm_joint_5" type="revolute">
|
||||
<parent link="arm_link_4"/>
|
||||
<child link="arm_link_5"/>
|
||||
<dynamics damping="3.0" friction="0.3"/>
|
||||
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.132"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
<link name="arm_link_5">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.124"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.248"/>
|
||||
</geometry>
|
||||
<material name="omni/Blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.124"/>
|
||||
<geometry>
|
||||
<box size="0.0356 0.05 0.248"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<!-- CENTER OF MASS -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0.124"/>
|
||||
<mass value="0.15813986"/>
|
||||
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
|
||||
<inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="arm_joint_6" type="fixed">
|
||||
<parent link="arm_link_5"/>
|
||||
<child link="arm_link_6"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.248"/>
|
||||
</joint>
|
||||
<link name="arm_link_6">
|
||||
<inertial>
|
||||
<!-- CENTER OF MASS -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="1e-12"/>
|
||||
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
|
||||
<inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- END OF ARM LINKS/JOINTS -->
|
||||
</robot>
|
||||
@@ -1,307 +0,0 @@
|
||||
<robot name="robot">
|
||||
<link name="base_footprint"></link>
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.5 0.5 0.01" />
|
||||
</geometry>
|
||||
<material name="base_link-material">
|
||||
<color rgba="0 0.6038273388475408 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.5 0.5 0.01" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_0_Joint" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="Axis_0" />
|
||||
<origin xyz="0 0 0.035" rpy="0 0 0" />
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/> </joint>
|
||||
<link name="Axis_0">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.15" length="0.059" />
|
||||
</geometry>
|
||||
<material name="Axis_0-material">
|
||||
<color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.15" length="0.059" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_1_Joint" type="revolute">
|
||||
<parent link="Axis_0" />
|
||||
<child link="Axis_1" />
|
||||
<origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/> </joint>
|
||||
<link name="Axis_1">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.082" length="0.1" />
|
||||
</geometry>
|
||||
<material name="Axis_1-material">
|
||||
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.082" length="0.1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_1_to_Segment_1" type="fixed">
|
||||
<parent link="Axis_1" />
|
||||
<child link="Segment_1" />
|
||||
<origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
|
||||
</joint>
|
||||
<link name="Segment_1">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.025" length="0.473" />
|
||||
</geometry>
|
||||
<material name="Segment_1-material">
|
||||
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.025" length="0.473" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_2_Joint" type="revolute">
|
||||
<parent link="Segment_1" />
|
||||
<child link="Axis_2" />
|
||||
<origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/> </joint>
|
||||
<link name="Axis_2">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.055" length="0.1" />
|
||||
</geometry>
|
||||
<material name="Axis_2-material">
|
||||
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.055" length="0.1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_2_to_Segment_2" type="fixed">
|
||||
<parent link="Axis_2" />
|
||||
<child link="Segment_2" />
|
||||
<origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
|
||||
</joint>
|
||||
<link name="Segment_2">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.025" length="0.393" />
|
||||
</geometry>
|
||||
<material name="Segment_2-material">
|
||||
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.025" length="0.393" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_3_Joint" type="revolute">
|
||||
<parent link="Segment_2" />
|
||||
<child link="Axis_3" />
|
||||
<origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/> </joint>
|
||||
<link name="Axis_3">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="0.1" />
|
||||
</geometry>
|
||||
<material name="Axis_3-material">
|
||||
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="0.1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_3_to_Segment_3" type="fixed">
|
||||
<parent link="Axis_3" />
|
||||
<child link="Segment_3" />
|
||||
<origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
|
||||
</joint>
|
||||
<link name="Segment_3">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.025" length="0.135" />
|
||||
</geometry>
|
||||
<material name="Segment_3-material">
|
||||
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.025" length="0.135" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Wrist_Joint" type="revolute">
|
||||
<parent link="Segment_3" />
|
||||
<child link="Axis_4" />
|
||||
<origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/> </joint>
|
||||
<link name="Axis_4">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.03" length="0.075" />
|
||||
</geometry>
|
||||
<material name="Axis_4-material">
|
||||
<color rgba="0.23455058215026167 0.9301108583738498 0.21952619971859377 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.03" length="0.075" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Continuous_Joint" type="revolute">
|
||||
<parent link="Axis_4" />
|
||||
<child link="Axis_4_C" />
|
||||
<origin xyz="0.0009533507860803557 0 0" rpy="0 -1.5707963267948966 0" />
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
|
||||
</joint>
|
||||
<link name="Axis_4_C">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.02" length="0.01" />
|
||||
</geometry>
|
||||
<material name="Axis_4_C-material">
|
||||
<color rgba="0.006048833020386069 0.407240211891531 0.15592646369776456 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.02" length="0.01" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="Axis_4_C_to_Effector" type="fixed">
|
||||
<parent link="Axis_4_C" />
|
||||
<child link="Effector" />
|
||||
<origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="Effector">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.05 0.01 0.135" />
|
||||
</geometry>
|
||||
<material name="Effector-material">
|
||||
<color rgba="0.2746773120495699 0.01680737574872402 0.5711248294565854 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.135" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
1
src/astra_msgs
Submodule
1
src/astra_msgs
Submodule
Submodule src/astra_msgs added at 6a57072723
@@ -9,51 +9,56 @@ import time
|
||||
import atexit
|
||||
import signal
|
||||
from std_msgs.msg import String
|
||||
from ros2_interfaces_pkg.msg import BioControl
|
||||
from ros2_interfaces_pkg.msg import BioFeedback
|
||||
from astra_msgs.msg import BioControl
|
||||
from astra_msgs.msg import BioFeedback
|
||||
|
||||
serial_pub = None
|
||||
thread = None
|
||||
|
||||
|
||||
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.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)
|
||||
|
||||
self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
|
||||
self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)
|
||||
|
||||
# Create subscribers
|
||||
self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10)
|
||||
self.control_sub = self.create_subscription(
|
||||
BioControl, "/bio/control", self.send_control, 10
|
||||
)
|
||||
|
||||
# Create a publisher for telemetry
|
||||
self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
|
||||
|
||||
# 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)
|
||||
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)
|
||||
|
||||
self.bio_feedback = BioFeedback()
|
||||
|
||||
|
||||
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode
|
||||
if self.launch_mode == 'bio':
|
||||
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
|
||||
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}...")
|
||||
# print(f"Checking port {port}...")
|
||||
ser.write(b"ping\n")
|
||||
response = ser.read_until("\n")
|
||||
|
||||
@@ -66,7 +71,9 @@ class SerialRelay(Node):
|
||||
pass
|
||||
|
||||
if self.port is None:
|
||||
self.get_logger().info("Unable to find MCU... please make sure it is connected.")
|
||||
self.get_logger().info(
|
||||
"Unable to find MCU... please make sure it is connected."
|
||||
)
|
||||
time.sleep(1)
|
||||
sys.exit(1)
|
||||
|
||||
@@ -78,11 +85,11 @@ class SerialRelay(Node):
|
||||
thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
|
||||
thread.start()
|
||||
|
||||
#if in arm mode, will need to read from the MCU
|
||||
# if in arm mode, will need to read from the MCU
|
||||
|
||||
try:
|
||||
while rclpy.ok():
|
||||
if self.launch_mode == 'bio':
|
||||
if self.launch_mode == "bio":
|
||||
if self.ser.in_waiting:
|
||||
self.read_mcu()
|
||||
else:
|
||||
@@ -92,8 +99,7 @@ class SerialRelay(Node):
|
||||
finally:
|
||||
self.cleanup()
|
||||
|
||||
|
||||
#Currently will just spit out all values over the /arm/feedback/debug topic as strings
|
||||
# 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")
|
||||
@@ -123,34 +129,48 @@ class SerialRelay(Node):
|
||||
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"
|
||||
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"
|
||||
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"
|
||||
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)
|
||||
# self.send_cmd(command)
|
||||
|
||||
# Vibration Motor
|
||||
command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
|
||||
#self.send_cmd(command)
|
||||
|
||||
# self.send_cmd(command)
|
||||
|
||||
# FAERIE Control Commands
|
||||
################
|
||||
@@ -159,33 +179,35 @@ class SerialRelay(Node):
|
||||
|
||||
# Laser
|
||||
command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
|
||||
#self.send_cmd(command)
|
||||
# self.send_cmd(command)
|
||||
|
||||
# Drill (SCABBARD)
|
||||
command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
|
||||
#self.send_cmd(command)
|
||||
# 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
|
||||
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
|
||||
elif self.launch_mode == "bio": # if in standalone mode, send to MCU directly
|
||||
self.get_logger().info(f"[Bio to MCU] {msg}")
|
||||
self.ser.write(bytes(msg, "utf8"))
|
||||
|
||||
def anchor_feedback(self, msg: String):
|
||||
output = msg.data
|
||||
parts = str(output.strip()).split(",")
|
||||
#self.get_logger().info(f"[Bio Anchor] {msg.data}")
|
||||
# self.get_logger().info(f"[Bio Anchor] {msg.data}")
|
||||
|
||||
if output.startswith("can_relay_fromvic,citadel,54"): # bat, 12, 5, Voltage readings * 100
|
||||
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
|
||||
@@ -199,7 +221,7 @@ class SerialRelay(Node):
|
||||
@staticmethod
|
||||
def list_serial_ports():
|
||||
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
|
||||
#return glob.glob("/dev/tty[A-Za-z]*")
|
||||
# return glob.glob("/dev/tty[A-Za-z]*")
|
||||
|
||||
def cleanup(self):
|
||||
print("Cleaning up...")
|
||||
@@ -209,11 +231,13 @@ class SerialRelay(Node):
|
||||
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
|
||||
@@ -222,7 +246,10 @@ def main(args=None):
|
||||
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
|
||||
|
||||
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()
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>common_interfaces</depend>
|
||||
<depend>ros2_interfaces_pkg</depend>
|
||||
<depend>astra_msgs</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
|
||||
@@ -1,25 +1,22 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'bio_pkg'
|
||||
package_name = "bio_pkg"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
version="0.0.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer='tristan',
|
||||
maintainer_email='tristanmcginnis26@gmail.com',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
maintainer="tristan",
|
||||
maintainer_email="tristanmcginnis26@gmail.com",
|
||||
description="TODO: Package description",
|
||||
license="TODO: License declaration",
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'bio = bio_pkg.bio_node:main'
|
||||
],
|
||||
"console_scripts": ["bio = bio_pkg.bio_node:main"],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -13,14 +13,17 @@ import os
|
||||
|
||||
import importlib
|
||||
from std_msgs.msg import String
|
||||
from ros2_interfaces_pkg.msg import CoreControl
|
||||
from astra_msgs.msg import CoreControl
|
||||
|
||||
|
||||
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
|
||||
os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init()
|
||||
os.environ["SDL_AUDIODRIVER"] = (
|
||||
"dummy" # Force pygame to use a dummy audio driver before pygame.init()
|
||||
)
|
||||
|
||||
|
||||
max_speed = 90 #Max speed as a duty cycle percentage (1-100)
|
||||
max_speed = 90 # Max speed as a duty cycle percentage (1-100)
|
||||
|
||||
|
||||
class Headless(Node):
|
||||
def __init__(self):
|
||||
@@ -43,13 +46,15 @@ class Headless(Node):
|
||||
# Initialize the gamepad
|
||||
self.gamepad = pygame.joystick.Joystick(0)
|
||||
self.gamepad.init()
|
||||
print(f'Gamepad Found: {self.gamepad.get_name()}')
|
||||
print(f"Gamepad Found: {self.gamepad.get_name()}")
|
||||
|
||||
# Now initialize the ROS2 node
|
||||
super().__init__("core_headless")
|
||||
self.create_timer(0.15, self.send_controls)
|
||||
self.publisher = self.create_publisher(CoreControl, '/core/control', 10)
|
||||
self.lastMsg = String() # Used to ignore sending controls repeatedly when they do not change
|
||||
self.publisher = self.create_publisher(CoreControl, "/core/control", 10)
|
||||
self.lastMsg = (
|
||||
String()
|
||||
) # Used to ignore sending controls repeatedly when they do not change
|
||||
|
||||
def run(self):
|
||||
# This thread makes all the update processes run in the background
|
||||
@@ -91,15 +96,17 @@ class Headless(Node):
|
||||
else:
|
||||
input.left_stick = -1 * round(self.gamepad.get_axis(1), 2) # left y-axis
|
||||
|
||||
output = f'L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}'
|
||||
output = f"L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}"
|
||||
self.get_logger().info(f"[Ctrl] {output}")
|
||||
self.publisher.publish(input)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = Headless()
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -19,8 +19,8 @@ from math import copysign, pi
|
||||
from std_msgs.msg import String, Header
|
||||
from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
|
||||
from geometry_msgs.msg import TwistStamped, Twist
|
||||
from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback, RevMotorState
|
||||
from ros2_interfaces_pkg.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
|
||||
from astra_msgs.msg import CoreControl, CoreFeedback, RevMotorState
|
||||
from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
|
||||
|
||||
|
||||
serial_pub = None
|
||||
@@ -38,7 +38,7 @@ control_qos = qos.QoSProfile(
|
||||
deadline=Duration(seconds=1),
|
||||
lifespan=Duration(nanoseconds=500_000_000), # 500ms
|
||||
liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
|
||||
liveliness_lease_duration=Duration(seconds=5)
|
||||
liveliness_lease_duration=Duration(seconds=5),
|
||||
)
|
||||
|
||||
# Used to verify the length of an incoming VicCAN feedback message
|
||||
@@ -52,7 +52,7 @@ viccan_msg_len_dict = {
|
||||
53: 4,
|
||||
54: 4,
|
||||
56: 4, # really 3, but viccan
|
||||
58: 4 # ditto
|
||||
58: 4, # ditto
|
||||
}
|
||||
|
||||
|
||||
@@ -62,77 +62,110 @@ class SerialRelay(Node):
|
||||
super().__init__("core_node")
|
||||
|
||||
# Launch mode -- anchor vs core
|
||||
self.declare_parameter('launch_mode', 'core')
|
||||
self.launch_mode = self.get_parameter('launch_mode').value
|
||||
self.declare_parameter("launch_mode", "core")
|
||||
self.launch_mode = self.get_parameter("launch_mode").value
|
||||
self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
|
||||
|
||||
|
||||
##################################################
|
||||
# Topics
|
||||
|
||||
# Anchor
|
||||
if self.launch_mode == 'anchor':
|
||||
self.anchor_fromvic_sub_ = self.create_subscription(VicCAN, '/anchor/from_vic/core', self.relay_fromvic, 20)
|
||||
self.anchor_tovic_pub_ = self.create_publisher(VicCAN, '/anchor/to_vic/relay', 20)
|
||||
if self.launch_mode == "anchor":
|
||||
self.anchor_fromvic_sub_ = self.create_subscription(
|
||||
VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
|
||||
)
|
||||
self.anchor_tovic_pub_ = self.create_publisher(
|
||||
VicCAN, "/anchor/to_vic/relay", 20
|
||||
)
|
||||
|
||||
self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
|
||||
self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
|
||||
self.anchor_sub = self.create_subscription(
|
||||
String, "/anchor/core/feedback", self.anchor_feedback, 10
|
||||
)
|
||||
self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
|
||||
|
||||
# Control
|
||||
|
||||
# autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
|
||||
self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, 1)
|
||||
self.cmd_vel_sub_ = self.create_subscription(
|
||||
TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
|
||||
)
|
||||
# manual twist -- [-1, 1] rather than real units
|
||||
self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos)
|
||||
self.twist_man_sub_ = self.create_subscription(
|
||||
Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
|
||||
)
|
||||
# manual flags -- brake mode and max duty cycle
|
||||
self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos)
|
||||
self.twist_max_duty = 0.5 # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
|
||||
self.control_state_sub_ = self.create_subscription(
|
||||
CoreCtrlState,
|
||||
"/core/control/state",
|
||||
self.control_state_callback,
|
||||
qos_profile=control_qos,
|
||||
)
|
||||
self.twist_max_duty = (
|
||||
0.5 # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
|
||||
)
|
||||
|
||||
# Feedback
|
||||
|
||||
# Consolidated and organized core feedback
|
||||
self.feedback_new_pub_ = self.create_publisher(NewCoreFeedback, '/core/feedback_new', qos_profile=qos.qos_profile_sensor_data)
|
||||
self.feedback_new_pub_ = self.create_publisher(
|
||||
NewCoreFeedback,
|
||||
"/core/feedback_new",
|
||||
qos_profile=qos.qos_profile_sensor_data,
|
||||
)
|
||||
self.feedback_new_state = NewCoreFeedback()
|
||||
self.feedback_new_state.fl_motor.id = 1
|
||||
self.feedback_new_state.bl_motor.id = 2
|
||||
self.feedback_new_state.fr_motor.id = 3
|
||||
self.feedback_new_state.br_motor.id = 4
|
||||
self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback) # TODO: not sure about this
|
||||
self.telemetry_pub_timer = self.create_timer(
|
||||
1.0, self.publish_feedback
|
||||
) # TODO: not sure about this
|
||||
# Joint states for topic-based controller
|
||||
self.joint_state_pub_ = self.create_publisher(JointState, '/core/joint_states', qos_profile=qos.qos_profile_sensor_data)
|
||||
self.joint_state_pub_ = self.create_publisher(
|
||||
JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
|
||||
)
|
||||
# IMU (embedded BNO-055)
|
||||
self.imu_pub_ = self.create_publisher(Imu, '/core/imu', qos_profile=qos.qos_profile_sensor_data)
|
||||
self.imu_pub_ = self.create_publisher(
|
||||
Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
|
||||
)
|
||||
self.imu_state = Imu()
|
||||
self.imu_state.header.frame_id = "core_bno055"
|
||||
# GPS (embedded u-blox M9N)
|
||||
self.gps_pub_ = self.create_publisher(NavSatFix, '/gps/fix', qos_profile=qos.qos_profile_sensor_data)
|
||||
self.gps_pub_ = self.create_publisher(
|
||||
NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
|
||||
)
|
||||
self.gps_state = NavSatFix()
|
||||
self.gps_state.header.frame_id = "core_gps_antenna"
|
||||
self.gps_state.status.service = NavSatStatus.SERVICE_GPS
|
||||
self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
|
||||
self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
|
||||
# Barometer (embedded BMP-388)
|
||||
self.baro_pub_ = self.create_publisher(Barometer, '/core/baro', qos_profile=qos.qos_profile_sensor_data)
|
||||
self.baro_pub_ = self.create_publisher(
|
||||
Barometer, "/core/baro", qos_profile=qos.qos_profile_sensor_data
|
||||
)
|
||||
self.baro_state = Barometer()
|
||||
self.baro_state.header.frame_id = "core_bmp388"
|
||||
|
||||
# Old
|
||||
|
||||
# /core/control
|
||||
self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
|
||||
self.control_sub = self.create_subscription(
|
||||
CoreControl, "/core/control", self.send_controls, 10
|
||||
) # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
|
||||
# /core/feedback
|
||||
self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
|
||||
self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
|
||||
self.core_feedback = CoreFeedback()
|
||||
# Debug
|
||||
self.debug_pub = self.create_publisher(String, '/core/debug', 10)
|
||||
self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
|
||||
|
||||
self.debug_pub = self.create_publisher(String, "/core/debug", 10)
|
||||
self.ping_service = self.create_service(
|
||||
Empty, "/astra/core/ping", self.ping_callback
|
||||
)
|
||||
|
||||
##################################################
|
||||
# Find microcontroller (Non-anchor only)
|
||||
|
||||
# Core (non-anchor) specific
|
||||
if self.launch_mode == 'core':
|
||||
if self.launch_mode == "core":
|
||||
# Loop through all serial devices on the computer to check for the MCU
|
||||
self.port = None
|
||||
ports = SerialRelay.list_serial_ports()
|
||||
@@ -141,7 +174,7 @@ class SerialRelay(Node):
|
||||
try:
|
||||
# connect and send a ping command
|
||||
ser = serial.Serial(port, 115200, timeout=1)
|
||||
#(f"Checking port {port}...")
|
||||
# (f"Checking port {port}...")
|
||||
ser.write(b"ping\n")
|
||||
response = ser.read_until("\n") # type: ignore
|
||||
|
||||
@@ -166,7 +199,6 @@ class SerialRelay(Node):
|
||||
atexit.register(self.cleanup)
|
||||
# end __init__()
|
||||
|
||||
|
||||
def run(self):
|
||||
# This thread makes all the update processes run in the background
|
||||
global thread
|
||||
@@ -175,7 +207,7 @@ class SerialRelay(Node):
|
||||
|
||||
try:
|
||||
while rclpy.ok():
|
||||
if self.launch_mode == 'core':
|
||||
if self.launch_mode == "core":
|
||||
self.read_MCU() # Check the MCU for updates
|
||||
except KeyboardInterrupt:
|
||||
sys.exit(0)
|
||||
@@ -213,8 +245,8 @@ class SerialRelay(Node):
|
||||
def scale_duty(self, value: float, max_speed: float):
|
||||
leftMin = -1
|
||||
leftMax = 1
|
||||
rightMin = -max_speed/100.0
|
||||
rightMax = max_speed/100.0
|
||||
rightMin = -max_speed / 100.0
|
||||
rightMax = max_speed / 100.0
|
||||
|
||||
# Figure out how 'wide' each range is
|
||||
leftSpan = leftMax - leftMin
|
||||
@@ -227,17 +259,29 @@ class SerialRelay(Node):
|
||||
return str(rightMin + (valueScaled * rightSpan))
|
||||
|
||||
def send_controls(self, msg: CoreControl):
|
||||
if(msg.turn_to_enable):
|
||||
command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n'
|
||||
if msg.turn_to_enable:
|
||||
command = (
|
||||
"can_relay_tovic,core,41,"
|
||||
+ str(msg.turn_to)
|
||||
+ ","
|
||||
+ str(msg.turn_to_timeout)
|
||||
+ "\n"
|
||||
)
|
||||
else:
|
||||
command = "can_relay_tovic,core,19," + self.scale_duty(msg.left_stick, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
|
||||
command = (
|
||||
"can_relay_tovic,core,19,"
|
||||
+ self.scale_duty(msg.left_stick, msg.max_speed)
|
||||
+ ","
|
||||
+ self.scale_duty(msg.right_stick, msg.max_speed)
|
||||
+ "\n"
|
||||
)
|
||||
self.send_cmd(command)
|
||||
|
||||
# Brake mode
|
||||
command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
|
||||
command = "can_relay_tovic,core,18," + str(int(msg.brake)) + "\n"
|
||||
self.send_cmd(command)
|
||||
|
||||
#print(f"[Sys] Relaying: {command}")
|
||||
# print(f"[Sys] Relaying: {command}")
|
||||
|
||||
def cmd_vel_callback(self, msg: TwistStamped):
|
||||
linear = msg.twist.linear.x
|
||||
@@ -255,13 +299,15 @@ class SerialRelay(Node):
|
||||
linear = msg.linear.x # [-1 1] for forward/back from left stick y
|
||||
angular = msg.angular.z # [-1 1] for left/right from right stick x
|
||||
|
||||
if (linear < 0): # reverse turning direction when going backwards (WIP)
|
||||
if linear < 0: # reverse turning direction when going backwards (WIP)
|
||||
angular *= -1
|
||||
|
||||
if abs(linear) > 1 or abs(angular) > 1:
|
||||
# if speed is greater than 1, then there is a problem
|
||||
# make it look like a problem and don't just run away lmao
|
||||
linear = copysign(0.25, linear) # 0.25 duty cycle in direction of control (hopefully slow)
|
||||
linear = copysign(
|
||||
0.25, linear
|
||||
) # 0.25 duty cycle in direction of control (hopefully slow)
|
||||
angular = copysign(0.25, angular)
|
||||
|
||||
duty_left = linear - angular
|
||||
@@ -272,8 +318,12 @@ class SerialRelay(Node):
|
||||
|
||||
# Apply max duty cycle
|
||||
# Joysticks provide values [-1, 1] rather than real units
|
||||
duty_left = map_range(duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty)
|
||||
duty_right = map_range(duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty)
|
||||
duty_left = map_range(
|
||||
duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty
|
||||
)
|
||||
duty_right = map_range(
|
||||
duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty
|
||||
)
|
||||
|
||||
self.send_viccan(19, [duty_left, duty_right])
|
||||
|
||||
@@ -285,24 +335,24 @@ class SerialRelay(Node):
|
||||
self.twist_max_duty = msg.max_duty # twist_man_callback will handle this
|
||||
|
||||
def send_cmd(self, msg: str):
|
||||
if self.launch_mode == 'anchor':
|
||||
#self.get_logger().info(f"[Core to Anchor Relay] {msg}")
|
||||
output = String()#Convert to std_msg string
|
||||
if self.launch_mode == "anchor":
|
||||
# self.get_logger().info(f"[Core to Anchor Relay] {msg}")
|
||||
output = String() # Convert to std_msg string
|
||||
output.data = msg
|
||||
self.anchor_pub.publish(output)
|
||||
elif self.launch_mode == 'core':
|
||||
elif self.launch_mode == "core":
|
||||
self.get_logger().info(f"[Core to MCU] {msg}")
|
||||
self.ser.write(bytes(msg, "utf8"))
|
||||
|
||||
|
||||
def send_viccan(self, cmd_id: int, data: list[float]):
|
||||
self.anchor_tovic_pub_.publish(VicCAN(
|
||||
self.anchor_tovic_pub_.publish(
|
||||
VicCAN(
|
||||
header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
|
||||
mcu_name="core",
|
||||
command_id=cmd_id,
|
||||
data=data
|
||||
))
|
||||
|
||||
data=data,
|
||||
)
|
||||
)
|
||||
|
||||
def anchor_feedback(self, msg: String):
|
||||
output = msg.data
|
||||
@@ -366,7 +416,7 @@ class SerialRelay(Node):
|
||||
return
|
||||
self.feedback_new_state.header.stamp = self.get_clock().now().to_msg()
|
||||
self.feedback_new_pub_.publish(self.feedback_new_state)
|
||||
#self.get_logger().info(f"[Core Anchor] {msg}")
|
||||
# self.get_logger().info(f"[Core Anchor] {msg}")
|
||||
|
||||
def relay_fromvic(self, msg: VicCAN):
|
||||
# Assume that the message is coming from Core
|
||||
@@ -376,7 +426,9 @@ class SerialRelay(Node):
|
||||
if msg.command_id in viccan_msg_len_dict:
|
||||
expected_len = viccan_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)})")
|
||||
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:
|
||||
@@ -386,7 +438,11 @@ class SerialRelay(Node):
|
||||
case 49: # GNSS Longitude
|
||||
self.gps_state.longitude = float(msg.data[0])
|
||||
case 50: # GNSS Satellite count and altitude
|
||||
self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
|
||||
self.gps_state.status.status = (
|
||||
NavSatStatus.STATUS_FIX
|
||||
if int(msg.data[0]) >= 3
|
||||
else NavSatStatus.STATUS_NO_FIX
|
||||
)
|
||||
self.gps_state.altitude = float(msg.data[1])
|
||||
self.gps_state.header.stamp = msg.header.stamp
|
||||
self.gps_pub_.publish(self.gps_state)
|
||||
@@ -402,7 +458,7 @@ class SerialRelay(Node):
|
||||
self.imu_state.linear_acceleration.y = float(msg.data[1])
|
||||
self.imu_state.linear_acceleration.z = float(msg.data[2])
|
||||
# Deal with quaternion
|
||||
r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
|
||||
r = Rotation.from_euler("z", float(msg.data[3]), degrees=True)
|
||||
q = r.as_quat()
|
||||
self.imu_state.orientation.x = q[0]
|
||||
self.imu_state.orientation.y = q[1]
|
||||
@@ -427,7 +483,9 @@ class SerialRelay(Node):
|
||||
case 4:
|
||||
motor = self.feedback_new_state.br_motor
|
||||
case _:
|
||||
self.get_logger().warning(f"Ignoring REV motor feedback 53 with invalid motorId {motorId}")
|
||||
self.get_logger().warning(
|
||||
f"Ignoring REV motor feedback 53 with invalid motorId {motorId}"
|
||||
)
|
||||
return
|
||||
|
||||
if motor:
|
||||
@@ -455,9 +513,15 @@ class SerialRelay(Node):
|
||||
motorId = round(float(msg.data[0]))
|
||||
position = float(msg.data[1])
|
||||
velocity = float(msg.data[2])
|
||||
joint_state_msg = JointState() # TODO: not sure if all motors should be in each message or not
|
||||
joint_state_msg.position = [position * (2 * pi) / CORE_GEAR_RATIO] # revolutions to radians
|
||||
joint_state_msg.velocity = [velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO] # RPM to rad/s
|
||||
joint_state_msg = (
|
||||
JointState()
|
||||
) # TODO: not sure if all motors should be in each message or not
|
||||
joint_state_msg.position = [
|
||||
position * (2 * pi) / CORE_GEAR_RATIO
|
||||
] # revolutions to radians
|
||||
joint_state_msg.velocity = [
|
||||
velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO
|
||||
] # RPM to rad/s
|
||||
|
||||
motor: RevMotorState | None = None
|
||||
|
||||
@@ -475,7 +539,9 @@ class SerialRelay(Node):
|
||||
motor = self.feedback_new_state.br_motor
|
||||
joint_state_msg.name = ["br_motor_joint"]
|
||||
case _:
|
||||
self.get_logger().warning(f"Ignoring REV motor feedback 58 with invalid motorId {motorId}")
|
||||
self.get_logger().warning(
|
||||
f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
|
||||
)
|
||||
return
|
||||
|
||||
joint_state_msg.header.stamp = msg.header.stamp
|
||||
@@ -483,15 +549,13 @@ class SerialRelay(Node):
|
||||
case _:
|
||||
return
|
||||
|
||||
|
||||
def publish_feedback(self):
|
||||
#self.get_logger().info(f"[Core] {self.core_feedback}")
|
||||
# self.get_logger().info(f"[Core] {self.core_feedback}")
|
||||
self.feedback_pub.publish(self.core_feedback)
|
||||
|
||||
def ping_callback(self, request, response):
|
||||
return response
|
||||
|
||||
|
||||
@staticmethod
|
||||
def list_serial_ports():
|
||||
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
|
||||
@@ -510,7 +574,10 @@ def myexcepthook(type, value, tb):
|
||||
if serial_pub:
|
||||
serial_pub.cleanup()
|
||||
|
||||
def map_range(value: float, in_min: float, in_max: float, out_min: float, out_max: float):
|
||||
|
||||
def map_range(
|
||||
value: float, in_min: float, in_max: float, out_min: float, out_max: float
|
||||
):
|
||||
return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
|
||||
|
||||
|
||||
@@ -523,7 +590,10 @@ def main(args=None):
|
||||
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
|
||||
|
||||
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()
|
||||
|
||||
@@ -9,7 +9,7 @@ import threading
|
||||
import time
|
||||
|
||||
from std_msgs.msg import String
|
||||
from ros2_interfaces_pkg.msg import PtzControl, PtzFeedback
|
||||
from astra_msgs.msg import PtzControl, PtzFeedback
|
||||
|
||||
# Import the SIYI SDK
|
||||
from core_pkg.siyi_sdk import (
|
||||
@@ -18,37 +18,43 @@ from core_pkg.siyi_sdk import (
|
||||
DataStreamType,
|
||||
DataStreamFrequency,
|
||||
SingleAxis,
|
||||
AttitudeData
|
||||
AttitudeData,
|
||||
)
|
||||
|
||||
|
||||
class PtzNode(Node):
|
||||
def __init__(self):
|
||||
# Initialize node with name
|
||||
super().__init__("core_ptz")
|
||||
|
||||
# Declare parameters
|
||||
self.declare_parameter('camera_ip', '192.168.1.9')
|
||||
self.declare_parameter('camera_port', 37260)
|
||||
self.declare_parameter("camera_ip", "192.168.1.9")
|
||||
self.declare_parameter("camera_port", 37260)
|
||||
|
||||
# Get parameters
|
||||
self.camera_ip = self.get_parameter('camera_ip').value
|
||||
self.camera_port = self.get_parameter('camera_port').value
|
||||
self.camera_ip = self.get_parameter("camera_ip").value
|
||||
self.camera_port = self.get_parameter("camera_port").value
|
||||
|
||||
self.get_logger().info(f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}")
|
||||
self.get_logger().info(
|
||||
f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}"
|
||||
)
|
||||
|
||||
# Create a camera instance
|
||||
self.camera = None
|
||||
self.camera_connected = False # This flag is still managed but not used to gate commands
|
||||
self.camera_connected = (
|
||||
False # This flag is still managed but not used to gate commands
|
||||
)
|
||||
self.loop = None
|
||||
self.thread_pool = None
|
||||
|
||||
# Create publishers
|
||||
self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
|
||||
self.debug_pub = self.create_publisher(String, '/ptz/debug', 10)
|
||||
self.feedback_pub = self.create_publisher(PtzFeedback, "/ptz/feedback", 10)
|
||||
self.debug_pub = self.create_publisher(String, "/ptz/debug", 10)
|
||||
|
||||
# Create subscribers
|
||||
self.control_sub = self.create_subscription(
|
||||
PtzControl, '/ptz/control', self.handle_control_command, 10)
|
||||
PtzControl, "/ptz/control", self.handle_control_command, 10
|
||||
)
|
||||
|
||||
# Create timers
|
||||
self.connection_timer = self.create_timer(5.0, self.check_camera_connection)
|
||||
@@ -57,7 +63,9 @@ class PtzNode(Node):
|
||||
|
||||
# Create feedback message
|
||||
self.feedback_msg = PtzFeedback()
|
||||
self.feedback_msg.connected = False # This will reflect the actual connection state
|
||||
self.feedback_msg.connected = (
|
||||
False # This will reflect the actual connection state
|
||||
)
|
||||
self.feedback_msg.error_msg = "Initializing"
|
||||
|
||||
# Flags for async operations
|
||||
@@ -86,8 +94,7 @@ class PtzNode(Node):
|
||||
|
||||
# Request attitude data stream
|
||||
await self.camera.send_data_stream_request(
|
||||
DataStreamType.ATTITUDE_DATA,
|
||||
DataStreamFrequency.HZ_10
|
||||
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_10
|
||||
)
|
||||
|
||||
# Update connection status
|
||||
@@ -108,8 +115,12 @@ class PtzNode(Node):
|
||||
# Update last_data_time regardless of self.camera_connected,
|
||||
# as data might arrive during a brief reconnect window.
|
||||
self.last_data_time = time.time()
|
||||
if self.camera_connected: # Only process for feedback if we believe we are connected
|
||||
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
|
||||
if (
|
||||
self.camera_connected
|
||||
): # Only process for feedback if we believe we are connected
|
||||
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
|
||||
data, AttitudeData
|
||||
):
|
||||
self.feedback_msg.yaw = data.yaw
|
||||
self.feedback_msg.pitch = data.pitch
|
||||
self.feedback_msg.roll = data.roll
|
||||
@@ -130,7 +141,6 @@ class PtzNode(Node):
|
||||
debug_str += str(data)
|
||||
self.get_logger().debug(debug_str)
|
||||
|
||||
|
||||
def check_camera_connection(self):
|
||||
"""Periodically check camera connection and attempt to reconnect if needed."""
|
||||
if not self.camera_connected and not self.shutdown_requested:
|
||||
@@ -140,7 +150,9 @@ class PtzNode(Node):
|
||||
if self.camera.is_connected: # SDK's internal connection state
|
||||
self.run_async_func(self.camera.disconnect())
|
||||
except Exception as e:
|
||||
self.get_logger().debug(f"Error during pre-reconnect disconnect: {e}")
|
||||
self.get_logger().debug(
|
||||
f"Error during pre-reconnect disconnect: {e}"
|
||||
)
|
||||
# self.camera = None # Don't nullify here, connect_to_camera will re-assign or create new
|
||||
|
||||
self.connect_task = self.thread_pool.submit(
|
||||
@@ -152,7 +164,9 @@ class PtzNode(Node):
|
||||
if self.camera_connected: # Only check health if we think we are connected
|
||||
time_since_last_data = time.time() - self.last_data_time
|
||||
if time_since_last_data > 5.0:
|
||||
self.publish_debug(f"No camera data for {time_since_last_data:.1f}s, marking as disconnected.")
|
||||
self.publish_debug(
|
||||
f"No camera data for {time_since_last_data:.1f}s, marking as disconnected."
|
||||
)
|
||||
self.camera_connected = False
|
||||
self.feedback_msg.connected = False
|
||||
self.feedback_msg.error_msg = "Connection stale (no data)"
|
||||
@@ -162,18 +176,19 @@ class PtzNode(Node):
|
||||
"""Handle incoming control commands."""
|
||||
# Removed: if not self.camera_connected
|
||||
if not self.camera: # Still check if camera object exists
|
||||
self.get_logger().warning("Camera object not initialized, ignoring control command")
|
||||
self.get_logger().warning(
|
||||
"Camera object not initialized, ignoring control command"
|
||||
)
|
||||
return
|
||||
|
||||
self.thread_pool.submit(
|
||||
self.run_async_func,
|
||||
self.process_control_command(msg)
|
||||
)
|
||||
self.thread_pool.submit(self.run_async_func, self.process_control_command(msg))
|
||||
|
||||
async def process_control_command(self, msg):
|
||||
"""Process and send the control command to the camera."""
|
||||
if not self.camera:
|
||||
self.get_logger().error("Process control command called but camera object is None.")
|
||||
self.get_logger().error(
|
||||
"Process control command called but camera object is None."
|
||||
)
|
||||
return
|
||||
try:
|
||||
# The SDK's send_... methods will raise RuntimeError if not connected.
|
||||
@@ -186,27 +201,35 @@ class PtzNode(Node):
|
||||
if msg.control_mode == 0:
|
||||
turn_yaw = max(-100, min(100, int(msg.turn_yaw)))
|
||||
turn_pitch = max(-100, min(100, int(msg.turn_pitch)))
|
||||
self.get_logger().debug(f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}")
|
||||
self.get_logger().debug(
|
||||
f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}"
|
||||
)
|
||||
await self.camera.send_rotation_command(turn_yaw, turn_pitch)
|
||||
|
||||
elif msg.control_mode == 1:
|
||||
yaw = max(-135.0, min(135.0, msg.yaw))
|
||||
pitch = max(-90.0, min(90.0, msg.pitch))
|
||||
self.get_logger().debug(f"Attempting absolute angles: yaw={yaw}, pitch={pitch}")
|
||||
self.get_logger().debug(
|
||||
f"Attempting absolute angles: yaw={yaw}, pitch={pitch}"
|
||||
)
|
||||
await self.camera.send_attitude_angles_command(yaw, pitch)
|
||||
|
||||
elif msg.control_mode == 2:
|
||||
axis = SingleAxis.YAW if msg.axis_id == 0 else SingleAxis.PITCH
|
||||
angle = msg.angle
|
||||
self.get_logger().debug(f"Attempting single axis: axis={axis.name}, angle={angle}")
|
||||
self.get_logger().debug(
|
||||
f"Attempting single axis: axis={axis.name}, angle={angle}"
|
||||
)
|
||||
await self.camera.send_single_axis_attitude_command(angle, axis)
|
||||
|
||||
elif msg.control_mode == 3:
|
||||
zoom_level = msg.zoom_level
|
||||
self.get_logger().debug(f"Attempting absolute zoom: level={zoom_level}x")
|
||||
self.get_logger().debug(
|
||||
f"Attempting absolute zoom: level={zoom_level}x"
|
||||
)
|
||||
await self.camera.send_absolute_zoom_command(zoom_level)
|
||||
|
||||
if hasattr(msg, 'stream_type') and hasattr(msg, 'stream_freq'):
|
||||
if hasattr(msg, "stream_type") and hasattr(msg, "stream_freq"):
|
||||
if msg.stream_type > 0 and msg.stream_freq >= 0:
|
||||
try:
|
||||
stream_type = DataStreamType(msg.stream_type)
|
||||
@@ -214,9 +237,13 @@ class PtzNode(Node):
|
||||
self.get_logger().info(
|
||||
f"Attempting to set data stream: type={stream_type.name}, freq={stream_freq.name}"
|
||||
)
|
||||
await self.camera.send_data_stream_request(stream_type, stream_freq)
|
||||
await self.camera.send_data_stream_request(
|
||||
stream_type, stream_freq
|
||||
)
|
||||
except ValueError:
|
||||
self.get_logger().error("Invalid stream type or frequency values in control message")
|
||||
self.get_logger().error(
|
||||
"Invalid stream type or frequency values in control message"
|
||||
)
|
||||
|
||||
except RuntimeError as e: # Catch SDK's "not connected" errors
|
||||
self.get_logger().warning(f"SDK command failed (likely not connected): {e}")
|
||||
@@ -231,7 +258,9 @@ class PtzNode(Node):
|
||||
def publish_debug(self, message_text):
|
||||
"""Publish debug message."""
|
||||
msg = String()
|
||||
msg.data = f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
|
||||
msg.data = (
|
||||
f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
|
||||
)
|
||||
self.debug_pub.publish(msg)
|
||||
self.get_logger().info(message_text)
|
||||
|
||||
@@ -239,18 +268,23 @@ class PtzNode(Node):
|
||||
"""Run an async function in the event loop."""
|
||||
if self.loop and self.loop.is_running():
|
||||
try:
|
||||
return asyncio.run_coroutine_threadsafe(coro, self.loop).result(timeout=5.0) # Added timeout
|
||||
return asyncio.run_coroutine_threadsafe(coro, self.loop).result(
|
||||
timeout=5.0
|
||||
) # Added timeout
|
||||
except asyncio.TimeoutError:
|
||||
self.get_logger().warning(f"Async function {coro.__name__} timed out.")
|
||||
return None
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Exception in run_async_func for {coro.__name__}: {e}")
|
||||
self.get_logger().error(
|
||||
f"Exception in run_async_func for {coro.__name__}: {e}"
|
||||
)
|
||||
return None
|
||||
else:
|
||||
self.get_logger().warning("Asyncio loop not running, cannot execute coroutine.")
|
||||
self.get_logger().warning(
|
||||
"Asyncio loop not running, cannot execute coroutine."
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
async def shutdown_node_async(self):
|
||||
"""Perform clean shutdown of camera connection."""
|
||||
self.shutdown_requested = True
|
||||
@@ -259,8 +293,7 @@ class PtzNode(Node):
|
||||
try:
|
||||
self.get_logger().info("Disabling data stream...")
|
||||
await self.camera.send_data_stream_request(
|
||||
DataStreamType.ATTITUDE_DATA,
|
||||
DataStreamFrequency.DISABLE
|
||||
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
|
||||
)
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
@@ -287,10 +320,14 @@ class PtzNode(Node):
|
||||
if self.loop and self.thread_pool:
|
||||
if self.loop.is_running():
|
||||
try:
|
||||
future = asyncio.run_coroutine_threadsafe(self.shutdown_node_async(), self.loop)
|
||||
future = asyncio.run_coroutine_threadsafe(
|
||||
self.shutdown_node_async(), self.loop
|
||||
)
|
||||
future.result(timeout=5)
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Error during async shutdown in cleanup: {e}")
|
||||
self.get_logger().error(
|
||||
f"Error during async shutdown in cleanup: {e}"
|
||||
)
|
||||
|
||||
self.get_logger().info("Shutting down thread pool executor...")
|
||||
self.thread_pool.shutdown(wait=True)
|
||||
@@ -301,7 +338,9 @@ class PtzNode(Node):
|
||||
|
||||
self.get_logger().info("PTZ node resources cleaned up.")
|
||||
else:
|
||||
self.get_logger().warning("Loop or thread_pool not initialized, skipping parts of cleanup.")
|
||||
self.get_logger().warning(
|
||||
"Loop or thread_pool not initialized, skipping parts of cleanup."
|
||||
)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
@@ -312,6 +351,7 @@ def main(args=None):
|
||||
|
||||
asyncio_thread = None
|
||||
if ptz_node.loop:
|
||||
|
||||
def run_event_loop(loop):
|
||||
asyncio.set_event_loop(loop)
|
||||
try:
|
||||
@@ -324,9 +364,7 @@ def main(args=None):
|
||||
loop.close()
|
||||
|
||||
asyncio_thread = threading.Thread(
|
||||
target=run_event_loop,
|
||||
args=(ptz_node.loop,),
|
||||
daemon=True
|
||||
target=run_event_loop, args=(ptz_node.loop,), daemon=True
|
||||
)
|
||||
asyncio_thread.start()
|
||||
|
||||
@@ -351,5 +389,5 @@ def main(args=None):
|
||||
ptz_node.get_logger().info("ROS shutdown complete.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -110,9 +110,7 @@ class SiyiGimbalCamera:
|
||||
|
||||
MAX_A8_MINI_ZOOM = 6.0 # Maximum zoom for A8 mini
|
||||
|
||||
def __init__(
|
||||
self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2
|
||||
):
|
||||
def __init__(self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2):
|
||||
self.ip = ip
|
||||
self.port = port
|
||||
self.heartbeat_interval = heartbeat_interval
|
||||
@@ -124,9 +122,7 @@ class SiyiGimbalCamera:
|
||||
|
||||
async def connect(self) -> None:
|
||||
try:
|
||||
self.reader, self.writer = await asyncio.open_connection(
|
||||
self.ip, self.port
|
||||
)
|
||||
self.reader, self.writer = await asyncio.open_connection(self.ip, self.port)
|
||||
self.is_connected = True
|
||||
asyncio.create_task(self.heartbeat_loop())
|
||||
asyncio.create_task(self._data_stream_listener())
|
||||
@@ -158,9 +154,7 @@ class SiyiGimbalCamera:
|
||||
if self.is_connected:
|
||||
await self.disconnect()
|
||||
|
||||
def _build_packet_header(
|
||||
self, cmd_id: CommandID, data_len: int
|
||||
) -> bytearray:
|
||||
def _build_packet_header(self, cmd_id: CommandID, data_len: int) -> bytearray:
|
||||
"""Helper to build the common packet header."""
|
||||
packet = bytearray()
|
||||
packet.extend(b"\x55\x66") # STX
|
||||
@@ -179,15 +173,11 @@ class SiyiGimbalCamera:
|
||||
|
||||
def _build_rotation_packet(self, turn_yaw: int, turn_pitch: int) -> bytes:
|
||||
data_len = 2
|
||||
packet = self._build_packet_header(
|
||||
CommandID.ROTATION_CONTROL, data_len
|
||||
)
|
||||
packet = self._build_packet_header(CommandID.ROTATION_CONTROL, data_len)
|
||||
packet.extend(struct.pack("bb", turn_yaw, turn_pitch))
|
||||
return self._finalize_packet(packet)
|
||||
|
||||
async def send_rotation_command(
|
||||
self, turn_yaw: int, turn_pitch: int
|
||||
) -> None:
|
||||
async def send_rotation_command(self, turn_yaw: int, turn_pitch: int) -> None:
|
||||
if not self.is_connected or not self.writer:
|
||||
raise RuntimeError(
|
||||
"Socket is not connected or writer is None, cannot send rotation command."
|
||||
@@ -199,21 +189,15 @@ class SiyiGimbalCamera:
|
||||
f"Sent rotation command with yaw_speed {turn_yaw} and pitch_speed {turn_pitch}"
|
||||
)
|
||||
|
||||
def _build_attitude_angles_packet(
|
||||
self, yaw: float, pitch: float
|
||||
) -> bytes:
|
||||
def _build_attitude_angles_packet(self, yaw: float, pitch: float) -> bytes:
|
||||
data_len = 4
|
||||
packet = self._build_packet_header(
|
||||
CommandID.ATTITUDE_ANGLES, data_len
|
||||
)
|
||||
packet = self._build_packet_header(CommandID.ATTITUDE_ANGLES, data_len)
|
||||
yaw_int = int(round(yaw * 10))
|
||||
pitch_int = int(round(pitch * 10))
|
||||
packet.extend(struct.pack("<hh", yaw_int, pitch_int))
|
||||
return self._finalize_packet(packet)
|
||||
|
||||
async def send_attitude_angles_command(
|
||||
self, yaw: float, pitch: float
|
||||
) -> None:
|
||||
async def send_attitude_angles_command(self, yaw: float, pitch: float) -> None:
|
||||
if not self.is_connected or not self.writer:
|
||||
raise RuntimeError(
|
||||
"Socket is not connected or writer is None, cannot send attitude angles command."
|
||||
@@ -221,17 +205,13 @@ class SiyiGimbalCamera:
|
||||
packet = self._build_attitude_angles_packet(yaw, pitch)
|
||||
self.writer.write(packet)
|
||||
await self.writer.drain()
|
||||
logger.debug(
|
||||
f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°"
|
||||
)
|
||||
logger.debug(f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°")
|
||||
|
||||
def _build_single_axis_attitude_packet(
|
||||
self, angle: float, axis: SingleAxis
|
||||
) -> bytes:
|
||||
data_len = 3
|
||||
packet = self._build_packet_header(
|
||||
CommandID.SINGLE_AXIS_CONTROL, data_len
|
||||
)
|
||||
packet = self._build_packet_header(CommandID.SINGLE_AXIS_CONTROL, data_len)
|
||||
angle_int = int(round(angle * 10))
|
||||
packet.extend(struct.pack("<hB", angle_int, axis.value))
|
||||
return self._finalize_packet(packet)
|
||||
@@ -254,9 +234,7 @@ class SiyiGimbalCamera:
|
||||
self, data_type: DataStreamType, data_freq: DataStreamFrequency
|
||||
) -> bytes:
|
||||
data_len = 2
|
||||
packet = self._build_packet_header(
|
||||
CommandID.DATA_STREAM_REQUEST, data_len
|
||||
)
|
||||
packet = self._build_packet_header(CommandID.DATA_STREAM_REQUEST, data_len)
|
||||
packet.append(data_type.value)
|
||||
packet.append(data_freq.value)
|
||||
return self._finalize_packet(packet)
|
||||
@@ -279,7 +257,9 @@ class SiyiGimbalCamera:
|
||||
data_len = 2
|
||||
packet = self._build_packet_header(CommandID.ABSOLUTE_ZOOM, data_len)
|
||||
zoom_packet_value = int(round(zoom_level * 10))
|
||||
if not (0 <= zoom_packet_value <= 65535): # Should be caught by clamping earlier
|
||||
if not (
|
||||
0 <= zoom_packet_value <= 65535
|
||||
): # Should be caught by clamping earlier
|
||||
raise ValueError(
|
||||
"Zoom packet value out of uint16_t range after conversion."
|
||||
)
|
||||
@@ -374,10 +354,7 @@ class SiyiGimbalCamera:
|
||||
self._data_callback(cmd_id_int, data)
|
||||
continue
|
||||
|
||||
if (
|
||||
cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE
|
||||
and len(data) == 12
|
||||
):
|
||||
if cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12:
|
||||
try:
|
||||
parsed = AttitudeData.from_bytes(data)
|
||||
if self._data_callback:
|
||||
@@ -385,9 +362,7 @@ class SiyiGimbalCamera:
|
||||
else:
|
||||
logger.info(f"Received attitude data: {parsed}")
|
||||
except Exception as e:
|
||||
logger.exception(
|
||||
f"Failed to parse attitude data: {e}"
|
||||
)
|
||||
logger.exception(f"Failed to parse attitude data: {e}")
|
||||
if self._data_callback:
|
||||
self._data_callback(cmd_id_enum, data)
|
||||
else:
|
||||
@@ -429,7 +404,9 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
|
||||
gimbal = SiyiGimbalCamera(gimbal_ip)
|
||||
|
||||
def my_data_handler(cmd_id, data):
|
||||
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
|
||||
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
|
||||
data, AttitudeData
|
||||
):
|
||||
print(
|
||||
f"Attitude: Yaw={data.yaw:.1f}, Pitch={data.pitch:.1f}, Roll={data.roll:.1f}"
|
||||
)
|
||||
@@ -470,16 +447,19 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
|
||||
await asyncio.sleep(2)
|
||||
|
||||
print("SDK Test: Requesting attitude data stream at 5Hz...")
|
||||
await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5)
|
||||
await gimbal.send_data_stream_request(
|
||||
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5
|
||||
)
|
||||
|
||||
print("SDK Test: Listening for data for 10 seconds...")
|
||||
await asyncio.sleep(10)
|
||||
|
||||
print("SDK Test: Disabling attitude data stream...")
|
||||
await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE)
|
||||
await gimbal.send_data_stream_request(
|
||||
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
|
||||
)
|
||||
await asyncio.sleep(1)
|
||||
|
||||
|
||||
except ConnectionRefusedError:
|
||||
print(
|
||||
f"SDK Test: Connection to {gimbal_ip} was refused. Is the gimbal on and accessible?"
|
||||
|
||||
@@ -24,7 +24,9 @@ async def main():
|
||||
await asyncio.sleep(2)
|
||||
|
||||
# Command 1: Move all the way to the right (using set angles)
|
||||
logger.info("Command 1: Move all the way to the right (using absolute angle control)")
|
||||
logger.info(
|
||||
"Command 1: Move all the way to the right (using absolute angle control)"
|
||||
)
|
||||
await camera.send_attitude_angles_command(135.0, 0.0)
|
||||
await asyncio.sleep(5)
|
||||
|
||||
@@ -35,13 +37,17 @@ async def main():
|
||||
await asyncio.sleep(5)
|
||||
|
||||
# Command 3: Stop looking down, then look up (with the single axis)
|
||||
logger.info("Command 3: Stop looking down and start looking up (single axis control)")
|
||||
logger.info(
|
||||
"Command 3: Stop looking down and start looking up (single axis control)"
|
||||
)
|
||||
await camera.send_rotation_command(0, 0)
|
||||
await camera.send_single_axis_attitude_command(135, SingleAxis.PITCH)
|
||||
await asyncio.sleep(5)
|
||||
|
||||
# Command 4: Reset and move all the way to the left (Absolute value).
|
||||
logger.info("Command 4: Move back to the center, and start moving all the way left")
|
||||
logger.info(
|
||||
"Command 4: Move back to the center, and start moving all the way left"
|
||||
)
|
||||
await camera.send_attitude_angles_command(-135.0, 0.0)
|
||||
await asyncio.sleep(5)
|
||||
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
<depend>common_interfaces</depend>
|
||||
<depend>python3-scipy</depend>
|
||||
<depend>python-crccheck-pip</depend>
|
||||
<depend>ros2_interfaces_pkg</depend>
|
||||
<depend>astra_msgs</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
|
||||
@@ -1,27 +1,26 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'core_pkg'
|
||||
package_name = "core_pkg"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
version="0.0.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer='tristan',
|
||||
maintainer_email='tristanmcginnis26@gmail.com',
|
||||
description='Core rover control package to handle command interpretation and embedded interfacing.',
|
||||
license='All Rights Reserved',
|
||||
maintainer="tristan",
|
||||
maintainer_email="tristanmcginnis26@gmail.com",
|
||||
description="Core rover control package to handle command interpretation and embedded interfacing.",
|
||||
license="All Rights Reserved",
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
"console_scripts": [
|
||||
"core = core_pkg.core_node:main",
|
||||
"headless = core_pkg.core_headless:main",
|
||||
"ptz = core_pkg.core_ptz:main"
|
||||
"ptz = core_pkg.core_ptz:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
<depend>rclpy</depend>
|
||||
<depend>common_interfaces</depend>
|
||||
<depend>python3-pygame</depend>
|
||||
<depend>ros2_interfaces_pkg</depend>
|
||||
<depend>astra_msgs</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
|
||||
@@ -1,23 +1,23 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'headless_pkg'
|
||||
package_name = "headless_pkg"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='1.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
version="1.0.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer='David Sharpe',
|
||||
maintainer_email='ds0196@uah.edu',
|
||||
description='Headless rover control package to handle command interpretation and embedded interfacing.',
|
||||
license='All Rights Reserved',
|
||||
maintainer="David Sharpe",
|
||||
maintainer_email="ds0196@uah.edu",
|
||||
description="Headless rover control package to handle command interpretation and embedded interfacing.",
|
||||
license="All Rights Reserved",
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
"console_scripts": [
|
||||
"headless_full = src.headless_node:main",
|
||||
],
|
||||
},
|
||||
|
||||
@@ -15,13 +15,15 @@ from math import copysign
|
||||
|
||||
from std_msgs.msg import String
|
||||
from geometry_msgs.msg import Twist
|
||||
from ros2_interfaces_pkg.msg import CoreControl, ArmManual, BioControl
|
||||
from ros2_interfaces_pkg.msg import CoreCtrlState
|
||||
from astra_msgs.msg import CoreControl, ArmManual, BioControl
|
||||
from astra_msgs.msg import CoreCtrlState
|
||||
|
||||
import pygame
|
||||
|
||||
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
|
||||
os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init()
|
||||
os.environ["SDL_AUDIODRIVER"] = (
|
||||
"dummy" # Force pygame to use a dummy audio driver before pygame.init()
|
||||
)
|
||||
|
||||
|
||||
CORE_STOP_MSG = CoreControl() # All zeros by default
|
||||
@@ -37,7 +39,7 @@ control_qos = qos.QoSProfile(
|
||||
deadline=Duration(seconds=1),
|
||||
lifespan=Duration(nanoseconds=500_000_000), # 500ms
|
||||
liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
|
||||
liveliness_lease_duration=Duration(seconds=5)
|
||||
liveliness_lease_duration=Duration(seconds=5),
|
||||
)
|
||||
|
||||
CORE_MODE = "twist" # "twist" or "duty"
|
||||
@@ -51,11 +53,11 @@ class Headless(Node):
|
||||
super().__init__("headless")
|
||||
|
||||
# Wait for anchor to start
|
||||
pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
|
||||
pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
|
||||
while len(pub_info) == 0:
|
||||
self.get_logger().info("Waiting for anchor to start...")
|
||||
time.sleep(1.0)
|
||||
pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
|
||||
pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
|
||||
|
||||
# Wait for a gamepad to be connected
|
||||
print("Waiting for gamepad connection...")
|
||||
@@ -71,17 +73,20 @@ class Headless(Node):
|
||||
# Initialize the gamepad
|
||||
self.gamepad = pygame.joystick.Joystick(0)
|
||||
self.gamepad.init()
|
||||
print(f'Gamepad Found: {self.gamepad.get_name()}')
|
||||
print(f"Gamepad Found: {self.gamepad.get_name()}")
|
||||
|
||||
self.create_timer(0.15, self.send_controls)
|
||||
|
||||
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.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos)
|
||||
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.core_state_pub_ = self.create_publisher(
|
||||
CoreCtrlState, "/core/control/state", qos_profile=control_qos
|
||||
)
|
||||
|
||||
self.ctrl_mode = "core" # Start in core mode
|
||||
self.core_brake_mode = False
|
||||
@@ -90,7 +95,6 @@ class Headless(Node):
|
||||
# Rumble when node is ready (returns False if rumble not supported)
|
||||
self.gamepad.rumble(0.7, 0.8, 150)
|
||||
|
||||
|
||||
def run(self):
|
||||
# This thread makes all the update processes run in the background
|
||||
thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
|
||||
@@ -104,7 +108,7 @@ class Headless(Node):
|
||||
sys.exit(0)
|
||||
|
||||
def send_controls(self):
|
||||
""" Read the gamepad state and publish control messages """
|
||||
"""Read the gamepad state and publish control messages"""
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
pygame.quit()
|
||||
@@ -122,7 +126,6 @@ class Headless(Node):
|
||||
pygame.quit()
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
new_ctrl_mode = self.ctrl_mode # if "" then inequality will always be true
|
||||
|
||||
# Check for control mode change
|
||||
@@ -137,7 +140,6 @@ class Headless(Node):
|
||||
self.ctrl_mode = new_ctrl_mode
|
||||
self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
|
||||
|
||||
|
||||
# CORE
|
||||
if self.ctrl_mode == "core" and CORE_MODE == "duty":
|
||||
input = CoreControl()
|
||||
@@ -148,7 +150,6 @@ class Headless(Node):
|
||||
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))
|
||||
|
||||
@@ -158,9 +159,8 @@ class Headless(Node):
|
||||
else:
|
||||
input.left_stick = float(round(-1 * left_stick_y, 2))
|
||||
|
||||
|
||||
# Debug
|
||||
output = f'L: {input.left_stick}, R: {input.right_stick}'
|
||||
output = f"L: {input.left_stick}, R: {input.right_stick}"
|
||||
self.get_logger().info(f"[Ctrl] {output}")
|
||||
|
||||
self.core_publisher.publish(input)
|
||||
@@ -179,13 +179,17 @@ class Headless(Node):
|
||||
|
||||
# Forward/back and Turn
|
||||
input.linear.x = -1.0 * left_stick_y
|
||||
input.angular.z = -1.0 * copysign(right_stick_x ** 2, right_stick_x) # Exponent for finer control (curve)
|
||||
input.angular.z = -1.0 * copysign(
|
||||
right_stick_x**2, right_stick_x
|
||||
) # Exponent for finer control (curve)
|
||||
|
||||
# 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)}")
|
||||
self.get_logger().info(
|
||||
f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
|
||||
)
|
||||
|
||||
# Brake mode
|
||||
new_brake_mode = button_a
|
||||
@@ -198,15 +202,19 @@ class Headless(Node):
|
||||
new_max_duty = 0.5
|
||||
|
||||
# Only publish if needed
|
||||
if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty:
|
||||
if (
|
||||
new_brake_mode != self.core_brake_mode
|
||||
or new_max_duty != self.core_max_duty
|
||||
):
|
||||
self.core_brake_mode = new_brake_mode
|
||||
self.core_max_duty = new_max_duty
|
||||
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}")
|
||||
|
||||
self.get_logger().info(
|
||||
f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}"
|
||||
)
|
||||
|
||||
# ARM and BIO
|
||||
if self.ctrl_mode == "arm":
|
||||
@@ -222,7 +230,6 @@ class Headless(Node):
|
||||
right_bumper = self.gamepad.get_button(5)
|
||||
dpad_input = self.gamepad.get_hat(0)
|
||||
|
||||
|
||||
# EF Grippers
|
||||
if left_trigger > 0 and right_trigger > 0:
|
||||
arm_input.gripper = 0
|
||||
@@ -237,7 +244,6 @@ class Headless(Node):
|
||||
elif dpad_input[0] == -1:
|
||||
arm_input.axis0 = -1
|
||||
|
||||
|
||||
if right_bumper: # Control end effector
|
||||
|
||||
# Effector yaw
|
||||
@@ -255,27 +261,28 @@ class Headless(Node):
|
||||
else: # Control arm axis
|
||||
|
||||
# Axis 1
|
||||
if abs(left_stick_x) > .15:
|
||||
if abs(left_stick_x) > 0.15:
|
||||
arm_input.axis1 = round(left_stick_x)
|
||||
|
||||
# Axis 2
|
||||
if abs(left_stick_y) > .15:
|
||||
if abs(left_stick_y) > 0.15:
|
||||
arm_input.axis2 = -1 * round(left_stick_y)
|
||||
|
||||
# Axis 3
|
||||
if abs(right_stick_y) > .15:
|
||||
if abs(right_stick_y) > 0.15:
|
||||
arm_input.axis3 = -1 * round(right_stick_y)
|
||||
|
||||
|
||||
# BIO
|
||||
bio_input = BioControl(
|
||||
bio_arm=int(left_stick_y * -100),
|
||||
drill_arm=int(round(right_stick_y) * -100)
|
||||
drill_arm=int(round(right_stick_y) * -100),
|
||||
)
|
||||
|
||||
# Drill motor (FAERIE)
|
||||
if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
|
||||
bio_input.drill = 100 * right_trigger - 100 * left_trigger
|
||||
bio_input.drill = int(
|
||||
30 * (right_trigger - left_trigger)
|
||||
) # Max duty cycle 30%
|
||||
|
||||
self.core_publisher.publish(CORE_STOP_MSG)
|
||||
self.arm_publisher.publish(arm_input)
|
||||
@@ -283,7 +290,7 @@ class Headless(Node):
|
||||
|
||||
|
||||
def deadzone(value: float, threshold=0.05) -> float:
|
||||
""" Apply a deadzone to a joystick input so the motors don't sound angry """
|
||||
"""Apply a deadzone to a joystick input so the motors don't sound angry"""
|
||||
if abs(value) < threshold:
|
||||
return 0
|
||||
return value
|
||||
@@ -295,6 +302,9 @@ def main(args=None):
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly
|
||||
|
||||
if __name__ == "__main__":
|
||||
signal.signal(
|
||||
signal.SIGTERM, lambda signum, frame: sys.exit(0)
|
||||
) # Catch termination signals and exit cleanly
|
||||
main()
|
||||
|
||||
Submodule src/ros2_interfaces_pkg deleted from d6806af6c3
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)
|
||||
Reference in New Issue
Block a user