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/arm_launch.py b/arm_launch.py
new file mode 100644
index 0000000..e17b321
--- /dev/null
+++ b/arm_launch.py
@@ -0,0 +1,91 @@
+#!/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 73f9316..e5aab92 100644
--- a/src/anchor_pkg/launch/rover.launch.py
+++ b/src/anchor_pkg/launch/rover.launch.py
@@ -2,7 +2,7 @@
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
@@ -18,13 +18,18 @@ 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}],
+ parameters=[{'launch_mode': mode},
+ {'robot_description': robot_description}],
on_exit=Shutdown()
)
)
diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py
index 66e2044..57ec6c5 100644
--- a/src/arm_pkg/arm_pkg/arm_node.py
+++ b/src/arm_pkg/arm_pkg/arm_node.py
@@ -13,6 +13,9 @@ 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
+from sensor_msgs.msg import JointState
+from tf2_ros import TransformBroadcaster, TransformStamped
+import math
# IK-Related imports
@@ -20,10 +23,6 @@ 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
@@ -64,6 +63,19 @@ class SerialRelay(Node):
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.tf_broadcaster = TransformBroadcaster(self)
+ self.joint_state = JointState()
+ self.joint_state.name = ['Axis_0', 'Axis_1_Joint', 'Axis_2_Joint', 'Axis_3_Joint', 'Continuous_Joint', 'Wrist_Joint']
+ self.joint_state.position = [0.0] * 6 # Initialize with zeros
+ self.odom_trans = TransformStamped()
+ self.odom_trans.header.frame_id = 'odom'
+ self.odom_trans.child_frame_id = 'base_footprint'
+ self.odom_trans.transform.translation.x = 0.0
+ self.odom_trans.transform.translation.y = 0.0
+ self.odom_trans.transform.translation.z = 0.0
+
# Topics used in anchor mode
if self.launch_mode == 'anchor':
self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
@@ -302,7 +314,6 @@ 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
@@ -319,11 +330,19 @@ class SerialRelay(Node):
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
+ self.joint_state.position[3] = math.radians(angles[3]) # Axis 3
+ self.joint_state.position[4] = math.radians(angles[4]) # Wrist roll
+ self.joint_state.position[5] = math.radians(angles[5]) # Wrist yaw
+ 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/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/urdf/Arm Base Plate.STL b/src/arm_pkg/urdf/Arm Base Plate.STL
new file mode 100755
index 0000000..5f6bafb
Binary files /dev/null and b/src/arm_pkg/urdf/Arm Base Plate.STL differ
diff --git a/src/arm_pkg/urdf/Axis 0 Plate.STL b/src/arm_pkg/urdf/Axis 0 Plate.STL
new file mode 100755
index 0000000..75ed3fa
Binary files /dev/null and b/src/arm_pkg/urdf/Axis 0 Plate.STL differ
diff --git a/src/arm_pkg/urdf/Axis 1.STL b/src/arm_pkg/urdf/Axis 1.STL
new file mode 100755
index 0000000..e620c77
Binary files /dev/null and b/src/arm_pkg/urdf/Axis 1.STL differ
diff --git a/src/arm_pkg/urdf/Segment 1.STL b/src/arm_pkg/urdf/Segment 1.STL
new file mode 100755
index 0000000..31be897
Binary files /dev/null and b/src/arm_pkg/urdf/Segment 1.STL differ
diff --git a/src/arm_pkg/urdf/arm12.urdf b/src/arm_pkg/urdf/arm12.urdf
index b63d872..e664ac1 100644
--- a/src/arm_pkg/urdf/arm12.urdf
+++ b/src/arm_pkg/urdf/arm12.urdf
@@ -1,22 +1,26 @@
+
+
+
-
+
-
+
+
-
+
@@ -27,24 +31,28 @@
-
+
+
-
+
-
-
+
+
+
+
-
+
-
+
+
-
+
@@ -55,17 +63,21 @@
+
-
+
-
+
+
+
-
+
-
+
+
@@ -83,19 +95,22 @@
+
+
-
+
-
+
+
-
+
@@ -110,12 +125,15 @@
+
-
+
+
+
@@ -138,11 +156,13 @@
+
+
@@ -165,12 +185,15 @@
+
-
+
+
+
@@ -193,11 +216,13 @@
+
+
@@ -220,12 +245,15 @@
+
-
+
+
+
@@ -248,6 +276,7 @@
+
@@ -255,6 +284,7 @@
+
@@ -277,11 +307,13 @@
+
+
@@ -304,4 +336,5 @@
+
\ No newline at end of file
diff --git a/src/arm_pkg/viz.rviz b/src/arm_pkg/viz.rviz
new file mode 100644
index 0000000..a7ee221
--- /dev/null
+++ b/src/arm_pkg/viz.rviz
@@ -0,0 +1,234 @@
+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