aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 01:18:09 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat: add viz code from Tristan's ik_test and add CAD to URDF

Browse Source
This commit is contained in:
David Sharpe
2025-08-17 09:46:56 -05:00
committed by David
parent 1281236b36
commit 2db9b67ebc
11 changed files with 417 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -14,3 +14,4 @@ __pycache__/
#Direnv
.direnv/
.venv

91
arm_launch.py Normal file
View File

@@ -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)
])

9
src/anchor_pkg/launch/rover.launch.py
View File

@@ -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()
)
)

39
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -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")

2
src/arm_pkg/setup.cfg
View File

@@ -2,3 +2,5 @@
script_dir=$base/lib/arm_pkg
[install]
install_scripts=$base/lib/arm_pkg
[build_scripts]
executable= /usr/bin/env python3

BIN
src/arm_pkg/urdf/Arm Base Plate.STL Executable file
View File

Binary file not shown.

BIN
src/arm_pkg/urdf/Axis 0 Plate.STL Executable file
View File

Binary file not shown.

BIN
src/arm_pkg/urdf/Axis 1.STL Executable file
View File

Binary file not shown.

BIN
src/arm_pkg/urdf/Segment 1.STL Executable file
View File

Binary file not shown.

73
src/arm_pkg/urdf/arm12.urdf
View File

@@ -1,22 +1,26 @@
<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" />
<origin xyz="-0.175 0 0" rpy="0 0 0" />
<geometry>
<box size="0.5 0.5 0.01" />
<!-- <box size="0.5 0.5 0.01" /> -->
<mesh filename="package://arm_pkg/urdf/Arm Base Plate.STL" scale="1 1 1"/>
</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" />
<origin xyz="-0.175 0 0" rpy="0 0 0" />
<geometry>
<box size="0.5 0.5 0.01" />
</geometry>
@@ -27,24 +31,28 @@
<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">
<joint name="Axis_0" type="revolute">
<parent link="base_link" />
<child link="Axis_0" />
<child link="Axis_0_Plate" />
<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">
<limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
</joint>
<link name="Axis_0_Plate">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<origin xyz="-0.175 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.15" length="0.059" />
<!-- <cylinder radius="0.15" length="0.059" /> -->
<mesh filename="package://arm_pkg/urdf/Axis 0 Plate.STL" scale="1 1 1"/>
</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" />
<origin xyz="-0.175 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.15" length="0.059" />
</geometry>
@@ -55,17 +63,21 @@
<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" />
<parent link="Axis_0_Plate" />
<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>
<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" />
<origin xyz="0 -0.11 0.005" rpy="-1.6 0 0" />
<geometry>
<cylinder radius="0.082" length="0.1" />
<!-- <cylinder radius="0.082" length="0.1" /> -->
<mesh filename="package://arm_pkg/urdf/Axis 1.STL" scale="1 1 1"/>
</geometry>
<material name="Axis_1-material">
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
@@ -83,19 +95,22 @@
<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" />
<origin xyz="-0.005 0 -0.35" rpy="0 0 0" />
<geometry>
<cylinder radius="0.025" length="0.473" />
<!-- <cylinder radius="0.025" length="0.473" /> -->
<mesh filename="package://arm_pkg/urdf/Segment 1.STL" scale="1 1 1"/>
</geometry>
<material name="Segment_1-material">
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
<color rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
@@ -110,12 +125,15 @@
<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>
<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" />
@@ -138,11 +156,13 @@
<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" />
@@ -165,12 +185,15 @@
<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>
<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" />
@@ -193,11 +216,13 @@
<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" />
@@ -220,12 +245,15 @@
<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>
<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" />
@@ -248,6 +276,7 @@
<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" />
@@ -255,6 +284,7 @@
<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" />
@@ -277,11 +307,13 @@
<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" />
@@ -304,4 +336,5 @@
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
</inertial>
</link>
</robot>

234
src/arm_pkg/viz.rviz Normal file
View File

@@ -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: <Fixed 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: <Fixed 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