diff --git a/arm_launch.py b/arm_launch.py
deleted file mode 100644
index e17b321..0000000
--- a/arm_launch.py
+++ /dev/null
@@ -1,91 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
-from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch.conditions import IfCondition
-
-
-
-#Prevent making __pycache__ directories
-from sys import dont_write_bytecode
-dont_write_bytecode = True
-
-def launch_setup(context, *args, **kwargs):
- # Retrieve the resolved value of the launch argument 'mode'
- mode = LaunchConfiguration('mode').perform(context)
- nodes = []
-
- # Arm
- # package_dir = get_package_share_directory('arm_pkg')
- package_dir = "/home/david/repos/rover-ros2/src/arm_pkg" # TODO: copy files to share and point there
- urdf_file_name = 'arm12.urdf'
- urdf = os.path.join(package_dir, "urdf", urdf_file_name)
- # urdf = "/home/david/repos/rover-ros2/src/arm_pkg/urdf/" + urdf_file_name
- with open(urdf, 'r') as infp:
- robot_desc = infp.read()
-
- rviz_config_path = os.path.join(package_dir, 'viz.rviz')
-
- if mode == 'anchor':
- nodes.append(
- Node(
- package='robot_state_publisher',
- executable='robot_state_publisher',
- name='robot_state_publisher',
- output='both',
- parameters=[{'robot_description': robot_desc}],
- arguments=[urdf],
- on_exit=Shutdown()
- )
- )
- nodes.append(
- Node(
- package='arm_pkg',
- executable='arm', # change as needed
- name='arm',
- output='both',
- parameters=[{'launch_mode': 'anchor'}],
- on_exit=Shutdown()
- )
- )
- nodes.append(
- Node(
- package='rviz2',
- executable='rviz2',
- name='rviz2',
- output='both',
- arguments=['-d', rviz_config_path],
- on_exit=Shutdown()
- )
- )
- # nodes.append(
- # Node(
- # package='anchor_pkg',
- # executable='anchor', # change as needed
- # name='anchor',
- # output='both',
- # parameters=[{'launch_mode': 'anchor'}],
- # on_exit=Shutdown()
- # )
- # )
- else:
- # If an invalid mode is provided, print an error.
- print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
-
- return nodes
-
-def generate_launch_description():
- declare_arg = DeclareLaunchArgument(
- 'mode',
- default_value='anchor',
- description='Launch mode: arm, core, bio, anchor, or ptz'
- )
-
- return LaunchDescription([
- declare_arg,
- OpaqueFunction(function=launch_setup)
- ])
diff --git a/src/anchor_pkg/launch/rover.launch.py b/src/anchor_pkg/launch/rover.launch.py
index e5aab92..5b82b82 100644
--- a/src/anchor_pkg/launch/rover.launch.py
+++ b/src/anchor_pkg/launch/rover.launch.py
@@ -6,7 +6,6 @@ from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoi
from launch_ros.actions import Node
-
#Prevent making __pycache__ directories
from sys import dont_write_bytecode
dont_write_bytecode = True
@@ -18,18 +17,14 @@ def launch_setup(context, *args, **kwargs):
if mode == 'anchor':
# Launch every node and pass "anchor" as the parameter
-
- # Arm
- urdf_file = "/home/david/repos/rover-ros2/src/arm_pkg/urdf/arm12.urdf"
- robot_description = {'robot_description': open(urdf_file).read()}
+
nodes.append(
Node(
package='arm_pkg',
executable='arm', # change as needed
name='arm',
output='both',
- parameters=[{'launch_mode': mode},
- {'robot_description': robot_description}],
+ parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
)
)
diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py
index ea63d4d..bd5dd41 100644
--- a/src/arm_pkg/arm_pkg/arm_node.py
+++ b/src/arm_pkg/arm_pkg/arm_node.py
@@ -12,7 +12,6 @@ from ros2_interfaces_pkg.msg import ArmManual
from ros2_interfaces_pkg.msg import SocketFeedback
from ros2_interfaces_pkg.msg import DigitFeedback
from sensor_msgs.msg import JointState
-from tf2_ros import TransformBroadcaster, TransformStamped
import math
# control_qos = qos.QoSProfile(
@@ -55,7 +54,6 @@ class SerialRelay(Node):
# New messages
self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
- self.tf_broadcaster = TransformBroadcaster(self)
self.joint_state = JointState()
self.joint_state.name = [
"Axis_0_Joint",
@@ -66,15 +64,7 @@ class SerialRelay(Node):
"Wrist-EF_Roll_Joint",
"Gripper_Slider_Left",
]
- self.joint_state.position = [0.0] * len(
- self.joint_state.name
- ) # Initialize with zeros
- self.odom_trans = TransformStamped()
- self.odom_trans.header.frame_id = "odom"
- self.odom_trans.child_frame_id = "base_link"
- self.odom_trans.transform.translation.x = 0.0
- self.odom_trans.transform.translation.y = 0.0
- self.odom_trans.transform.translation.z = 0.0
+ 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
@@ -179,10 +169,8 @@ class SerialRelay(Node):
# 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
- command += f"can_relay_tovic,digit,36,0,{positions[4]}\n"
- # Wrist roll
- command += f"can_relay_tovic,digit,35,{positions[5]}\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)
@@ -193,36 +181,18 @@ class SerialRelay(Node):
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"
- )
+ 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
- # 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)
@@ -263,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
@@ -280,18 +252,7 @@ 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]
- #
- #
- # 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]
@@ -302,13 +263,10 @@ class SerialRelay(Node):
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)
- self.joint_state.position[4] = math.radians(angles[4]) # Wrist roll
- self.joint_state.position[5] = math.radians(angles[5]) # Wrist yaw
+ # 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)
- # self.odom_trans.header.stamp = self.get_clock().now().to_msg()
- self.tf_broadcaster.sendTransform(self.odom_trans)
else:
self.get_logger().info("Invalid angle feedback input format")
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.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/Arm Base Plate.STL b/src/arm_pkg/urdf/Arm Base Plate.STL
deleted file mode 100755
index 5f6bafb..0000000
Binary files a/src/arm_pkg/urdf/Arm Base Plate.STL and /dev/null differ
diff --git a/src/arm_pkg/urdf/Axis 0 Plate.STL b/src/arm_pkg/urdf/Axis 0 Plate.STL
deleted file mode 100755
index 75ed3fa..0000000
Binary files a/src/arm_pkg/urdf/Axis 0 Plate.STL and /dev/null differ
diff --git a/src/arm_pkg/urdf/Axis 1.STL b/src/arm_pkg/urdf/Axis 1.STL
deleted file mode 100755
index e620c77..0000000
Binary files a/src/arm_pkg/urdf/Axis 1.STL and /dev/null differ
diff --git a/src/arm_pkg/urdf/Segment 1.STL b/src/arm_pkg/urdf/Segment 1.STL
deleted file mode 100755
index 31be897..0000000
Binary files a/src/arm_pkg/urdf/Segment 1.STL and /dev/null differ
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 8a21092..0000000
--- a/src/arm_pkg/urdf/arm12.urdf
+++ /dev/null
@@ -1,340 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/arm_pkg/viz.rviz b/src/arm_pkg/viz.rviz
deleted file mode 100644
index a7ee221..0000000
--- a/src/arm_pkg/viz.rviz
+++ /dev/null
@@ -1,234 +0,0 @@
-Panels:
- - Class: rviz_common/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- - /TF1
- - /RobotModel1
- - /RobotModel1/Description Topic1
- Splitter Ratio: 0.5
- Tree Height: 717
- - Class: rviz_common/Selection
- Name: Selection
- - Class: rviz_common/Tool Properties
- Expanded:
- - /2D Goal Pose1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz_common/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz_default_plugins/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 10
- Reference Frame:
- Value: true
- - Class: rviz_default_plugins/TF
- Enabled: true
- Frame Timeout: 15
- Frames:
- All Enabled: true
- arm_link_0:
- Value: true
- arm_link_1:
- Value: true
- arm_link_2:
- Value: true
- arm_link_3:
- Value: true
- arm_link_4:
- Value: true
- arm_link_5:
- Value: true
- arm_link_6:
- Value: true
- base_link:
- Value: true
- odom:
- Value: true
- Marker Scale: 0.44999998807907104
- Name: TF
- Show Arrows: true
- Show Axes: true
- Show Names: false
- Tree:
- odom:
- base_link:
- arm_link_0:
- arm_link_1:
- arm_link_2:
- arm_link_3:
- arm_link_4:
- arm_link_5:
- arm_link_6:
- {}
- Update Interval: 0
- Value: true
- - Alpha: 1
- Class: rviz_default_plugins/RobotModel
- Collision Enabled: false
- Description File: ""
- Description Source: Topic
- Description Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /robot_description
- Enabled: true
- Links:
- All Links Enabled: true
- Expand Joint Details: false
- Expand Link Details: false
- Expand Tree: false
- Link Tree Style: Links in Alphabetic Order
- arm_link_0:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm_link_1:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm_link_2:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm_link_3:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm_link_4:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm_link_5:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm_link_6:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Mass Properties:
- Inertia: false
- Mass: false
- Name: RobotModel
- TF Prefix: ""
- Update Interval: 0
- Value: true
- Visual Enabled: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: odom
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz_default_plugins/Interact
- Hide Inactive Objects: true
- - Class: rviz_default_plugins/MoveCamera
- - Class: rviz_default_plugins/Select
- - Class: rviz_default_plugins/FocusCamera
- - Class: rviz_default_plugins/Measure
- Line color: 128; 128; 0
- - Class: rviz_default_plugins/SetInitialPose
- Covariance x: 0.25
- Covariance y: 0.25
- Covariance yaw: 0.06853891909122467
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /initialpose
- - Class: rviz_default_plugins/SetGoal
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /goal_pose
- - Class: rviz_default_plugins/PublishPoint
- Single click: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /clicked_point
- Transformation:
- Current:
- Class: rviz_default_plugins/TF
- Value: true
- Views:
- Current:
- Class: rviz_default_plugins/Orbit
- Distance: 3.1002085208892822
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: 0
- Y: 0
- Z: 0
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.6997972726821899
- Target Frame:
- Value: Orbit (rviz)
- Yaw: 5.6954216957092285
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: true
- Height: 945
- Hide Left Dock: true
- Hide Right Dock: true
- QMainWindow State: 000000ff00000000fd0000000400000000000001630000036afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002c0000036a000000ab00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002effc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c000002ef000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e80000036a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: true
- Width: 1512
- X: 0
- Y: 37