diff --git a/.gitignore b/.gitignore
index 71add28..dab2965 100644
--- a/.gitignore
+++ b/.gitignore
@@ -14,3 +14,4 @@ __pycache__/
#Direnv
.direnv/
+.venv
diff --git a/auto_start/auto_start_anchor.sh b/auto_start/auto_start_anchor.sh
index 0d7e9c2..71cc2e1 100755
--- a/auto_start/auto_start_anchor.sh
+++ b/auto_start/auto_start_anchor.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash
# Wait for a network interface to be up (not necessarily online)
while ! ip link show | grep -q "state UP"; do
diff --git a/auto_start/auto_start_core_headless.sh b/auto_start/auto_start_core_headless.sh
index b59126b..8e787b8 100755
--- a/auto_start/auto_start_core_headless.sh
+++ b/auto_start/auto_start_core_headless.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash
# Wait for a network interface to be up (not necessarily online)
while ! ip link show | grep -q "state UP"; do
diff --git a/auto_start/auto_start_headless_full.sh b/auto_start/auto_start_headless_full.sh
index 488e033..870a646 100755
--- a/auto_start/auto_start_headless_full.sh
+++ b/auto_start/auto_start_headless_full.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash
# Wait for a network interface to be up (not necessarily online)
while ! ip link show | grep -q "state UP"; do
diff --git a/auto_start/start_rosbag.sh b/auto_start/start_rosbag.sh
index 3d659f0..786a153 100755
--- a/auto_start/start_rosbag.sh
+++ b/auto_start/start_rosbag.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash
ANCHOR_WS="/home/clucky/rover-ros2"
AUTONOMY_WS="/home/clucky/rover-Autonomy"
diff --git a/flake.nix b/flake.nix
index 8e7b60f..17dd3b9 100644
--- a/flake.nix
+++ b/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 :(
];
}
diff --git a/src/anchor_pkg/launch/rover.launch.py b/src/anchor_pkg/launch/rover.launch.py
index 73f9316..5b82b82 100644
--- a/src/anchor_pkg/launch/rover.launch.py
+++ b/src/anchor_pkg/launch/rover.launch.py
@@ -2,11 +2,10 @@
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
from sys import dont_write_bytecode
dont_write_bytecode = True
@@ -18,6 +17,7 @@ def launch_setup(context, *args, **kwargs):
if mode == 'anchor':
# Launch every node and pass "anchor" as the parameter
+
nodes.append(
Node(
package='arm_pkg',
diff --git a/src/arm_pkg/arm_pkg/arm_headless.py b/src/arm_pkg/arm_pkg/arm_headless.py
index bdce40b..4078e16 100755
--- a/src/arm_pkg/arm_pkg/arm_headless.py
+++ b/src/arm_pkg/arm_pkg/arm_headless.py
@@ -17,9 +17,12 @@ from ros2_interfaces_pkg.msg import ArmManual
from ros2_interfaces_pkg.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:
@@ -133,7 +131,7 @@ class Headless(Node):
elif self.gamepad.get_axis(3) < 0:
input.effector_roll = -1
- else: # Control arm axis
+ else: # Control arm axis
dpad_input = self.gamepad.get_hat(0)
input.axis0 = 0
if dpad_input[0] == 1:
@@ -141,44 +139,44 @@ 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
pass
-
+
# Depricated, kept temporarily for reference
# def send_controls(self):
@@ -190,7 +188,7 @@ class Headless(Node):
# input.lt = self.gamepad.get_axis(2)#left trigger
# input.rt = self.gamepad.get_axis(5)#right trigger
-
+
# #input.lb = self.gamepad.get_button(9)#Value must be converted to bool
# if(self.gamepad.get_button(4)):#left bumper
# input.lb = True
@@ -202,7 +200,7 @@ class Headless(Node):
# input.rb = True
# else:
# input.rb = False
-
+
# #input.plus = self.gamepad.get_button(6)#plus button
# if(self.gamepad.get_button(7)):#plus button
# input.plus = True
@@ -218,7 +216,7 @@ class Headless(Node):
# input.ls_x = round(self.gamepad.get_axis(0),2)#left x-axis
# input.ls_y = round(self.gamepad.get_axis(1),2)#left y-axis
# input.rs_x = round(self.gamepad.get_axis(3),2)#right x-axis
- # input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis
+ # input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis
# #input.a = self.gamepad.get_button(1)#A button
# if(self.gamepad.get_button(0)):#A button
@@ -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:
@@ -257,10 +253,9 @@ class Headless(Node):
# input.d_left = True
# else:
# input.d_left = False
-
# if pygame.joystick.get_count() != 0:
-
+
# self.get_logger().info(f"[Ctrl] Updated Controller State\n")
# self.publisher.publish(input)
@@ -268,20 +263,17 @@ class Headless(Node):
# pass
-
-
-
def main(args=None):
rclpy.init(args=args)
node = Headless()
-
+
rclpy.spin(node)
rclpy.shutdown()
- #tb_bs = BaseStation()
- #node.run()
+ # tb_bs = BaseStation()
+ # node.run()
-if __name__ == '__main__':
+if __name__ == "__main__":
main()
diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py
index 66e2044..bd5dd41 100644
--- a/src/arm_pkg/arm_pkg/arm_node.py
+++ b/src/arm_pkg/arm_pkg/arm_node.py
@@ -1,6 +1,5 @@
import rclpy
from rclpy.node import Node
-from rclpy import qos
import serial
import sys
import threading
@@ -10,22 +9,10 @@ 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 sensor_msgs.msg import JointState
+import math
# control_qos = qos.QoSProfile(
# history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -48,42 +35,62 @@ 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
@@ -96,12 +103,14 @@ class SerialRelay(Node):
pass
if self.port is not None:
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)
-
+
self.ser = serial.Serial(self.port, 115200)
atexit.register(self.cleanup)
@@ -109,12 +118,12 @@ class SerialRelay(Node):
global thread
thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
thread.start()
-
- #if in arm mode, will need to read from the MCU
+
+ # 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 +133,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 +160,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
-
- 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"
- 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 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]
+ # 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)
def send_manual(self, msg: ArmManual):
axis0 = msg.axis0
@@ -232,42 +180,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 end effector
+ # 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"
- # 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"
+ # Send controls for end effector
- command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
+ command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
- command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+ # command += f"can_relay_tovic,digit,26,{msg.gripper}\n" # no hardware rn
- command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n"
+ command += f"can_relay_tovic,digit,28,{msg.laser}\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 +233,8 @@ 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
@@ -296,34 +246,27 @@ class SerialRelay(Node):
# Angle feedbacks,
# split the msg.data by commas
parts = msg.split(",")
-
+
if len(parts) >= 7:
# Extract the angles from the string
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")
@@ -340,7 +283,7 @@ class SerialRelay(Node):
self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
else:
self.get_logger().info("Invalid voltage feedback input format")
-
+
def updateMotorFeedback(self, msg: str):
parts = str(msg.strip()).split(",")
motorId = round(float(parts[3]))
@@ -364,11 +307,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 +320,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 +335,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()
diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py
deleted file mode 100644
index 44a1373..0000000
--- a/src/arm_pkg/arm_pkg/astra_arm.py
+++ /dev/null
@@ -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
diff --git a/src/arm_pkg/package.xml b/src/arm_pkg/package.xml
index efcb547..373ca31 100644
--- a/src/arm_pkg/package.xml
+++ b/src/arm_pkg/package.xml
@@ -11,8 +11,6 @@
common_interfaces
python3-numpy
ros2_interfaces_pkg
-
- python3-ikpy-pip
ament_copyright
ament_flake8
diff --git a/src/arm_pkg/setup.cfg b/src/arm_pkg/setup.cfg
index ad255d7..796e974 100644
--- a/src/arm_pkg/setup.cfg
+++ b/src/arm_pkg/setup.cfg
@@ -2,3 +2,5 @@
script_dir=$base/lib/arm_pkg
[install]
install_scripts=$base/lib/arm_pkg
+[build_scripts]
+executable= /usr/bin/env python3
diff --git a/src/arm_pkg/setup.py b/src/arm_pkg/setup.py
index 089982a..5a2a266 100644
--- a/src/arm_pkg/setup.py
+++ b/src/arm_pkg/setup.py
@@ -6,14 +6,12 @@ package_name = 'arm_pkg'
setup(
name=package_name,
- version='0.0.0',
+ 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/' + package_name, ['package.xml'])
],
install_requires=['setuptools'],
zip_safe=True,
diff --git a/src/arm_pkg/urdf/arm11.urdf b/src/arm_pkg/urdf/arm11.urdf
deleted file mode 100644
index 2fb7036..0000000
--- a/src/arm_pkg/urdf/arm11.urdf
+++ /dev/null
@@ -1,239 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/arm_pkg/urdf/arm12.urdf b/src/arm_pkg/urdf/arm12.urdf
deleted file mode 100644
index b63d872..0000000
--- a/src/arm_pkg/urdf/arm12.urdf
+++ /dev/null
@@ -1,307 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/servo_arm_twist_pkg/CMakeLists.txt b/src/servo_arm_twist_pkg/CMakeLists.txt
new file mode 100644
index 0000000..8719d44
--- /dev/null
+++ b/src/servo_arm_twist_pkg/CMakeLists.txt
@@ -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()
diff --git a/src/servo_arm_twist_pkg/README.md b/src/servo_arm_twist_pkg/README.md
new file mode 100644
index 0000000..727ae50
--- /dev/null
+++ b/src/servo_arm_twist_pkg/README.md
@@ -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.
diff --git a/src/servo_arm_twist_pkg/package.xml b/src/servo_arm_twist_pkg/package.xml
new file mode 100644
index 0000000..c2d1601
--- /dev/null
+++ b/src/servo_arm_twist_pkg/package.xml
@@ -0,0 +1,58 @@
+
+
+
+ servo_arm_twist_pkg
+ 2.5.9
+ Provides real-time manipulator Cartesian and joint servoing.
+ Blake Anderson
+ Andy Zelenak
+ Tyler Weaver
+ Henning Kayser
+
+ BSD 3-Clause
+
+ https://ros-planning.github.io/moveit_tutorials
+
+ Brian O'Neil
+ Andy Zelenak
+ Blake Anderson
+ Alexander Rössler
+ Tyler Weaver
+ Adam Pettinger
+
+ ament_cmake
+ moveit_common
+
+ control_msgs
+ control_toolbox
+ geometry_msgs
+ moveit_msgs
+ moveit_core
+ moveit_ros_planning_interface
+ pluginlib
+ sensor_msgs
+ std_msgs
+ std_srvs
+ tf2_eigen
+ trajectory_msgs
+
+ gripper_controllers
+ joint_state_broadcaster
+ joint_trajectory_controller
+ joy
+ robot_state_publisher
+ tf2_ros
+ moveit_configs_utils
+ launch_param_builder
+
+ ament_cmake_gtest
+ ament_lint_auto
+ ament_lint_common
+ controller_manager
+ ros_testing
+
+
+ ament_cmake
+
+
+
diff --git a/src/servo_arm_twist_pkg/src/joystick_twist.cpp b/src/servo_arm_twist_pkg/src/joystick_twist.cpp
new file mode 100644
index 0000000..5a117d0
--- /dev/null
+++ b/src/servo_arm_twist_pkg/src/joystick_twist.cpp
@@ -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
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+// 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_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
+std::map 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& axes, const std::vector& buttons,
+ std::unique_ptr& twist,
+ std::unique_ptr& 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& 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(
+ JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
+ [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
+
+ twist_pub_ = this->create_publisher(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
+ joint_pub_ = this->create_publisher(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
+ // collision_pub_ =
+ // this->create_publisher("/planning_scene", rclcpp::SystemDefaultsQoS());
+
+ // Create a service client to start the ServoNode
+ servo_start_client_ = this->create_client("/servo_node/start_servo");
+ servo_start_client_->wait_for_service(std::chrono::seconds(1));
+ servo_start_client_->async_send_request(std::make_shared());
+
+ // // 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();
+ // 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();
+ auto joint_msg = std::make_unique();
+
+ // 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::SharedPtr joy_sub_;
+ rclcpp::Publisher::SharedPtr twist_pub_;
+ rclcpp::Publisher::SharedPtr joint_pub_;
+ rclcpp::Publisher::SharedPtr collision_pub_;
+ rclcpp::Client::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(servo_arm_twist_pkg::JoyToServoPub)