diff --git a/setup/draw_hitboxes.py b/setup/draw_hitboxes.py new file mode 100644 index 00000000..3975dcdd --- /dev/null +++ b/setup/draw_hitboxes.py @@ -0,0 +1,431 @@ +#!/usr/bin/env python3 + +""" +Use to draw simple box/cylinder hitboxes over an STL mesh, then export URDF blocks. +""" +import argparse +import math +import numpy as np +import trimesh + +from PyQt5 import QtWidgets, QtCore +import pyvista as pv +from pyvistaqt import QtInteractor + + +def rpy_to_R(roll, pitch, yaw): + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + + Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]], dtype=float) + Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]], dtype=float) + Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]], dtype=float) + return Rz @ Ry @ Rx + + +def make_T(xyz, rpy): + R = rpy_to_R(*rpy) + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = xyz + return T + + +def fmt(v): + return " ".join(f"{x:.6f}" for x in v) + + +class Primitive: + def __init__(self, prim_type="box"): + self.type = prim_type # "box" or "cyl" + self.xyz = np.array([0.0, 0.0, 0.0], dtype=float) + self.rpy = np.array([0.0, 0.0, 0.0], dtype=float) + + if prim_type == "box": + self.size = np.array([0.1, 0.1, 0.1], dtype=float) # x y z + self.radius = None + self.length = None + else: + self.radius = 0.05 + self.length = 0.2 + self.size = None + + def urdf_collision(self): + if self.type == "box": + return f""" + + + + + """ + else: + return f""" + + + + + """ + + def build_mesh(self): + if self.type == "box": + m = trimesh.creation.box(extents=self.size) + else: + m = trimesh.creation.cylinder( + radius=self.radius, height=self.length, sections=64 + ) + + T = make_T(self.xyz, self.rpy) + m.apply_transform(T) + return m + + +class HitboxEditor(QtWidgets.QMainWindow): + def __init__(self, stl_path, scale=1.0): + super().__init__() + self.setWindowTitle("Hitbox Drawer (Boxes/Cylinders) -> URDF Collisions") + self.resize(1400, 900) + + self.prims = [] + self.actors = [] # pyvista actors per primitive + self.selected = -1 + + # Load STL with trimesh, convert to pyvista + mesh = trimesh.load_mesh(stl_path, force="mesh") + if scale != 1.0: + mesh.apply_scale(scale) + self.mesh_trimesh = mesh + + pv_mesh = pv.wrap(mesh.as_open3d.triangle_mesh) if False else None # not used + # Create PyVista PolyData directly + verts = mesh.vertices + faces = np.hstack([np.full((mesh.faces.shape[0], 1), 3), mesh.faces]).astype( + np.int64 + ) + faces = faces.reshape(-1) + self.pv_stl = pv.PolyData(verts, faces) + + central = QtWidgets.QWidget() + self.setCentralWidget(central) + layout = QtWidgets.QHBoxLayout(central) + + # 3D viewer + self.plotter = QtInteractor(central) + layout.addWidget(self.plotter.interactor, stretch=3) + + # Right panel + panel = QtWidgets.QWidget() + layout.addWidget(panel, stretch=1) + pl = QtWidgets.QVBoxLayout(panel) + + # Buttons + btn_row = QtWidgets.QHBoxLayout() + self.btn_add_box = QtWidgets.QPushButton("New Box") + self.btn_add_cyl = QtWidgets.QPushButton("New Cylinder") + self.btn_del = QtWidgets.QPushButton("Delete") + btn_row.addWidget(self.btn_add_box) + btn_row.addWidget(self.btn_add_cyl) + btn_row.addWidget(self.btn_del) + pl.addLayout(btn_row) + + # Primitive list + self.listw = QtWidgets.QListWidget() + pl.addWidget(self.listw) + + # Form fields + form = QtWidgets.QFormLayout() + pl.addLayout(form) + + self.x = QtWidgets.QDoubleSpinBox() + self.x.setRange(-1000, 1000) + self.x.setDecimals(6) + self.x.setSingleStep(0.01) + self.y = QtWidgets.QDoubleSpinBox() + self.y.setRange(-1000, 1000) + self.y.setDecimals(6) + self.y.setSingleStep(0.01) + self.z = QtWidgets.QDoubleSpinBox() + self.z.setRange(-1000, 1000) + self.z.setDecimals(6) + self.z.setSingleStep(0.01) + + self.roll = QtWidgets.QDoubleSpinBox() + self.roll.setRange(-math.pi, math.pi) + self.roll.setDecimals(6) + self.roll.setSingleStep(0.01) + self.pitch = QtWidgets.QDoubleSpinBox() + self.pitch.setRange(-math.pi, math.pi) + self.pitch.setDecimals(6) + self.pitch.setSingleStep(0.01) + self.yaw = QtWidgets.QDoubleSpinBox() + self.yaw.setRange(-math.pi, math.pi) + self.yaw.setDecimals(6) + self.yaw.setSingleStep(0.01) + + self.sx = QtWidgets.QDoubleSpinBox() + self.sx.setRange(0.0001, 1000) + self.sx.setDecimals(6) + self.sx.setSingleStep(0.01) + self.sy = QtWidgets.QDoubleSpinBox() + self.sy.setRange(0.0001, 1000) + self.sy.setDecimals(6) + self.sy.setSingleStep(0.01) + self.sz = QtWidgets.QDoubleSpinBox() + self.sz.setRange(0.0001, 1000) + self.sz.setDecimals(6) + self.sz.setSingleStep(0.01) + + self.radius = QtWidgets.QDoubleSpinBox() + self.radius.setRange(0.0001, 1000) + self.radius.setDecimals(6) + self.radius.setSingleStep(0.01) + self.length = QtWidgets.QDoubleSpinBox() + self.length.setRange(0.0001, 1000) + self.length.setDecimals(6) + self.length.setSingleStep(0.01) + + form.addRow("x", self.x) + form.addRow("y", self.y) + form.addRow("z", self.z) + form.addRow("roll", self.roll) + form.addRow("pitch", self.pitch) + form.addRow("yaw", self.yaw) + form.addRow("box sx", self.sx) + form.addRow("box sy", self.sy) + form.addRow("box sz", self.sz) + form.addRow("cyl radius", self.radius) + form.addRow("cyl length", self.length) + + # Export + self.btn_export = QtWidgets.QPushButton("Copy URDF collisions to clipboard") + pl.addWidget(self.btn_export) + + # Viewer setup + self.plotter.add_mesh(self.pv_stl, opacity=0.25) + self.plotter.add_axes() + self.plotter.reset_camera() + + # Signals + self.btn_add_box.clicked.connect(self.add_box) + self.btn_add_cyl.clicked.connect(self.add_cyl) + self.btn_del.clicked.connect(self.delete_selected) + self.listw.currentRowChanged.connect(self.select_index) + + for w in [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + self.sx, + self.sy, + self.sz, + self.radius, + self.length, + ]: + w.valueChanged.connect(self.on_fields_changed) + + self.btn_export.clicked.connect(self.export_urdf) + + self.update_fields_enabled(False) + + def update_fields_enabled(self, enabled): + for w in [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + self.sx, + self.sy, + self.sz, + self.radius, + self.length, + ]: + w.setEnabled(enabled) + + def add_box(self): + p = Primitive("box") + # initialize roughly at mesh center + p.xyz = self.mesh_trimesh.bounding_box.centroid.copy() + # size roughly from mesh extents / 4 + ext = self.mesh_trimesh.bounding_box.extents + p.size = np.maximum(ext / 4.0, 0.02) + self.prims.append(p) + self.listw.addItem(f"box_{len(self.prims)-1}") + self.spawn_actor(len(self.prims) - 1) + self.listw.setCurrentRow(len(self.prims) - 1) + + def add_cyl(self): + p = Primitive("cyl") + p.xyz = self.mesh_trimesh.bounding_box.centroid.copy() + ext = self.mesh_trimesh.bounding_box.extents + p.length = float(max(ext) / 3.0) + p.radius = float(max(min(ext) / 6.0, 0.01)) + self.prims.append(p) + self.listw.addItem(f"cyl_{len(self.prims)-1}") + self.spawn_actor(len(self.prims) - 1) + self.listw.setCurrentRow(len(self.prims) - 1) + + def spawn_actor(self, idx): + prim = self.prims[idx] + tm = prim.build_mesh() + + verts = tm.vertices + faces = ( + np.hstack([np.full((tm.faces.shape[0], 1), 3), tm.faces]) + .astype(np.int64) + .reshape(-1) + ) + pd = pv.PolyData(verts, faces) + + actor = self.plotter.add_mesh(pd, opacity=0.35) + self.actors.append((pd, actor)) + self.plotter.render() + + def update_actor(self, idx): + prim = self.prims[idx] + tm = prim.build_mesh() + + pd, actor = self.actors[idx] + # Update points and faces (faces constant for box/cyl? safer to rebuild) + verts = tm.vertices + faces = ( + np.hstack([np.full((tm.faces.shape[0], 1), 3), tm.faces]) + .astype(np.int64) + .reshape(-1) + ) + new_pd = pv.PolyData(verts, faces) + + # Remove old actor and re-add (simple + reliable) + self.plotter.remove_actor(actor) + new_actor = self.plotter.add_mesh(new_pd, opacity=0.35) + self.actors[idx] = (new_pd, new_actor) + self.plotter.render() + + def delete_selected(self): + if self.selected < 0 or self.selected >= len(self.prims): + return + _, actor = self.actors[self.selected] + self.plotter.remove_actor(actor) + + del self.prims[self.selected] + del self.actors[self.selected] + self.listw.takeItem(self.selected) + + # Re-label list + for i in range(self.listw.count()): + t = self.listw.item(i).text() + kind = "box" if "box" in t else "cyl" + self.listw.item(i).setText(f"{kind}_{i}") + + self.selected = -1 + self.update_fields_enabled(False) + self.plotter.render() + + def select_index(self, idx): + self.selected = idx + if idx < 0 or idx >= len(self.prims): + self.update_fields_enabled(False) + return + + self.update_fields_enabled(True) + prim = self.prims[idx] + + # Block signals while populating fields + ws = [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + self.sx, + self.sy, + self.sz, + self.radius, + self.length, + ] + for w in ws: + w.blockSignals(True) + + self.x.setValue(float(prim.xyz[0])) + self.y.setValue(float(prim.xyz[1])) + self.z.setValue(float(prim.xyz[2])) + self.roll.setValue(float(prim.rpy[0])) + self.pitch.setValue(float(prim.rpy[1])) + self.yaw.setValue(float(prim.rpy[2])) + + if prim.type == "box": + self.sx.setValue(float(prim.size[0])) + self.sy.setValue(float(prim.size[1])) + self.sz.setValue(float(prim.size[2])) + else: + self.radius.setValue(float(prim.radius)) + self.length.setValue(float(prim.length)) + + # Enable/disable size fields based on type + is_box = prim.type == "box" + self.sx.setEnabled(is_box) + self.sy.setEnabled(is_box) + self.sz.setEnabled(is_box) + self.radius.setEnabled(not is_box) + self.length.setEnabled(not is_box) + + for w in ws: + w.blockSignals(False) + + def on_fields_changed(self): + if self.selected < 0 or self.selected >= len(self.prims): + return + prim = self.prims[self.selected] + + prim.xyz = np.array( + [self.x.value(), self.y.value(), self.z.value()], dtype=float + ) + prim.rpy = np.array( + [self.roll.value(), self.pitch.value(), self.yaw.value()], dtype=float + ) + + if prim.type == "box": + prim.size = np.array( + [self.sx.value(), self.sy.value(), self.sz.value()], dtype=float + ) + else: + prim.radius = float(self.radius.value()) + prim.length = float(self.length.value()) + + self.update_actor(self.selected) + + def export_urdf(self): + blocks = [p.urdf_collision() for p in self.prims] + out = "\n".join(blocks) + + cb = QtWidgets.QApplication.clipboard() + cb.setText(out) + + QtWidgets.QMessageBox.information( + self, "Copied", "URDF blocks copied to clipboard." + ) + + +def main(): + ap = argparse.ArgumentParser( + description="Interactive collision primitive drawer for an STL." + ) + ap.add_argument("stl", help="Path to STL file") + ap.add_argument( + "--scale", type=float, default=1.0, help="Scale factor (e.g. 0.001 for mm->m)" + ) + args = ap.parse_args() + + app = QtWidgets.QApplication([]) + w = HitboxEditor(args.stl, scale=args.scale) + w.show() + app.exec_() + + +if __name__ == "__main__": + main() diff --git a/src/Arm/arm_control/.setup_assistant b/src/Arm/arm_control/.setup_assistant new file mode 100644 index 00000000..571638a0 --- /dev/null +++ b/src/Arm/arm_control/.setup_assistant @@ -0,0 +1,23 @@ +moveit_setup_assistant_config: + urdf: + package: rover_urdf + relative_path: urdf/rover_urdf.urdf.xacro + srdf: + relative_path: config/rover_urdf.srdf + package_settings: + author_name: Connor Needham + author_email: connor.needham2015@gmail.com + generated_timestamp: 1767296302 + control_xacro: + command: + [] + state: + [] + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + [] + state: + [] \ No newline at end of file diff --git a/src/Arm/arm_srdf/CMakeLists.txt b/src/Arm/arm_control/CMakeLists.txt similarity index 94% rename from src/Arm/arm_srdf/CMakeLists.txt rename to src/Arm/arm_control/CMakeLists.txt index 882d6153..d8cb834b 100644 --- a/src/Arm/arm_srdf/CMakeLists.txt +++ b/src/Arm/arm_control/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(arm_srdf) +project(arm_control) find_package(ament_cmake REQUIRED) diff --git a/src/Arm/arm_srdf/config/arm_config.yaml b/src/Arm/arm_control/config/arm_config.yaml similarity index 93% rename from src/Arm/arm_srdf/config/arm_config.yaml rename to src/Arm/arm_control/config/arm_config.yaml index e8792ce8..1cafc4ac 100644 --- a/src/Arm/arm_srdf/config/arm_config.yaml +++ b/src/Arm/arm_control/config/arm_config.yaml @@ -26,8 +26,8 @@ scale: command_out_type: trajectory_msgs/JointTrajectory # What to publish? Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: false -publish_joint_velocities: true +publish_joint_positions: true +publish_joint_velocities: false publish_joint_accelerations: false #low_pass_filter_coeff: 2.0 @@ -43,9 +43,9 @@ is_primary_planning_scene_monitor: false check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) ## MoveIt properties -move_group_name: rover_arm # Often 'manipulator' or 'arm' +move_group_name: arm # Often 'manipulator' or 'arm' planning_frame: Link_1 -ee_frame_name: eef_link +ee_frame_name: EndEffector robot_link_command_frame: Link_1 ## Stopping behaviour @@ -70,10 +70,10 @@ cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian t joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states # Get joint states from this tpoic status_topic: ~/status # Publish status to this topic -command_out_topic: /rover_arm_controller/joint_trajectory # Publish outgoing commands here +command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body -check_collisions: false # Check collisions? +check_collisions: true # Check collisions? collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/src/Arm/arm_srdf/config/initial_positions.yaml b/src/Arm/arm_control/config/initial_positions.yaml similarity index 100% rename from src/Arm/arm_srdf/config/initial_positions.yaml rename to src/Arm/arm_control/config/initial_positions.yaml diff --git a/src/Arm/arm_srdf/config/joint_limits.yaml b/src/Arm/arm_control/config/joint_limits.yaml similarity index 100% rename from src/Arm/arm_srdf/config/joint_limits.yaml rename to src/Arm/arm_control/config/joint_limits.yaml diff --git a/src/Arm/arm_srdf/config/kinematics.yaml b/src/Arm/arm_control/config/kinematics.yaml similarity index 94% rename from src/Arm/arm_srdf/config/kinematics.yaml rename to src/Arm/arm_control/config/kinematics.yaml index d46c1ed9..2f6bb96b 100644 --- a/src/Arm/arm_srdf/config/kinematics.yaml +++ b/src/Arm/arm_control/config/kinematics.yaml @@ -1,4 +1,4 @@ -rover_arm: +arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/src/Arm/arm_srdf/config/moveit.rviz b/src/Arm/arm_control/config/moveit.rviz similarity index 97% rename from src/Arm/arm_srdf/config/moveit.rviz rename to src/Arm/arm_control/config/moveit.rviz index b9a3adbf..99bb740f 100644 --- a/src/Arm/arm_srdf/config/moveit.rviz +++ b/src/Arm/arm_control/config/moveit.rviz @@ -27,7 +27,7 @@ Visualization Manager: Robot Alpha: 0.5 Value: true Global Options: - Fixed Frame: Link_1 + Fixed Frame: base_link Tools: - Class: rviz_default_plugins/Interact - Class: rviz_default_plugins/MoveCamera @@ -43,7 +43,7 @@ Visualization Manager: Z: 0.30 Name: Current View Pitch: 0.5 - Target Frame: Link_1 + Target Frame: base_link Yaw: -0.623 Window Geometry: Height: 975 diff --git a/src/Arm/arm_srdf/config/moveit_controllers.yaml b/src/Arm/arm_control/config/moveit_controllers.yaml similarity index 88% rename from src/Arm/arm_srdf/config/moveit_controllers.yaml rename to src/Arm/arm_control/config/moveit_controllers.yaml index 4c45a3a6..d95643aa 100644 --- a/src/Arm/arm_srdf/config/moveit_controllers.yaml +++ b/src/Arm/arm_control/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - rover_arm_controller + - arm_controller - rover_arm_controller: + arm_controller: type: FollowJointTrajectory joints: - Joint_1 diff --git a/src/Arm/arm_srdf/config/pilz_cartesian_limits.yaml b/src/Arm/arm_control/config/pilz_cartesian_limits.yaml similarity index 100% rename from src/Arm/arm_srdf/config/pilz_cartesian_limits.yaml rename to src/Arm/arm_control/config/pilz_cartesian_limits.yaml diff --git a/src/Arm/arm_srdf/config/ros2_controllers.yaml b/src/Arm/arm_control/config/ros2_controllers.yaml similarity index 74% rename from src/Arm/arm_srdf/config/ros2_controllers.yaml rename to src/Arm/arm_control/config/ros2_controllers.yaml index 28684e07..39237beb 100644 --- a/src/Arm/arm_srdf/config/ros2_controllers.yaml +++ b/src/Arm/arm_control/config/ros2_controllers.yaml @@ -3,14 +3,14 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - rover_arm_controller: + arm_controller: type: joint_trajectory_controller/JointTrajectoryController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster -rover_arm_controller: +arm_controller: ros__parameters: joints: - Joint_1 @@ -20,9 +20,7 @@ rover_arm_controller: - Joint_5 - Joint_6 command_interfaces: - - velocity + - position state_interfaces: - position - - velocity - allow_integration_in_goal_trajectories: true - open_loop_control: true \ No newline at end of file + - velocity \ No newline at end of file diff --git a/src/Arm/arm_control/config/rover_urdf.srdf b/src/Arm/arm_control/config/rover_urdf.srdf new file mode 100644 index 00000000..3c4cc6e0 --- /dev/null +++ b/src/Arm/arm_control/config/rover_urdf.srdf @@ -0,0 +1,292 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Arm/arm_control/launch/move_group.launch.py b/src/Arm/arm_control/launch/move_group.launch.py new file mode 100644 index 00000000..573267c7 --- /dev/null +++ b/src/Arm/arm_control/launch/move_group.launch.py @@ -0,0 +1,9 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + "rover_urdf", package_name="arm_control" + ).to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/src/Arm/arm_srdf/launch/moveit_rviz.launch.py b/src/Arm/arm_control/launch/moveit_rviz.launch.py similarity index 85% rename from src/Arm/arm_srdf/launch/moveit_rviz.launch.py rename to src/Arm/arm_control/launch/moveit_rviz.launch.py index ce5d30a2..4cdb70ad 100644 --- a/src/Arm/arm_srdf/launch/moveit_rviz.launch.py +++ b/src/Arm/arm_control/launch/moveit_rviz.launch.py @@ -4,6 +4,6 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder( - "arm_urdf", package_name="arm_srdf" + "rover_urdf", package_name="arm_control" ).to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/src/Arm/arm_srdf/launch/servo.launch.py b/src/Arm/arm_control/launch/servo.launch.py similarity index 54% rename from src/Arm/arm_srdf/launch/servo.launch.py rename to src/Arm/arm_control/launch/servo.launch.py index bc17ce76..d00a1eb8 100644 --- a/src/Arm/arm_srdf/launch/servo.launch.py +++ b/src/Arm/arm_control/launch/servo.launch.py @@ -40,26 +40,16 @@ def arm_launch(moveit_config, launch_package_path=None) -> LaunchDescription: Launch a self-contained MoveIt demo. Includes: - * robot_state_publisher * move_group - * moveit_rviz + * moveit_rviz (Optional) * ros2_control_node + controller spawners """ if launch_package_path is None: launch_package_path = moveit_config.package_path ld = LaunchDescription() - - # Launch arguments - ld.add_action(DeclareBooleanLaunchArg("use_composition", default_value=True)) - ld.add_action( - DeclareBooleanLaunchArg("use_intra_process_comms", default_value=True) - ) ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=False)) - use_composition = LaunchConfiguration("use_composition") - use_intra_process_comms = LaunchConfiguration("use_intra_process_comms") - # Move group ld.add_action(generate_move_group_launch(moveit_config)) @@ -75,11 +65,10 @@ def arm_launch(moveit_config, launch_package_path=None) -> LaunchDescription: ) # Common parameters - servo_yaml = load_yaml("arm_srdf", "config/arm_config.yaml") + servo_yaml = load_yaml("arm_control", "config/arm_config.yaml") parameters = [ { "moveit_servo": servo_yaml, - "use_intra_process_comms": use_intra_process_comms, "publish_frequency": 15.0, }, moveit_config.robot_description, @@ -95,69 +84,29 @@ def arm_launch(moveit_config, launch_package_path=None) -> LaunchDescription: parameters=parameters, remappings=[("/controller_manager/robot_description", "/robot_description")], ) - - # Non-composed nodes - load_nodes = GroupAction( - condition=IfCondition(PythonExpression(["not ", use_composition])), - actions=[ - Node( - package="moveit_servo", - executable="servo_node_main", - parameters=parameters, - output="screen", - ), - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - parameters=parameters, - ), - ], - ) - - # Composable container - container_name = "arm_container" - container = ComposableNodeContainer( - namespace="", - package="rclcpp_components", - executable="component_container_isolated", - name=container_name, + servo_node = Node( + package="moveit_servo", + executable="servo_node_main", + parameters=parameters, output="screen", - condition=IfCondition(use_composition), - ) - - # Composable nodes (loaded into container when composition is enabled) - load_composable_nodes = LoadComposableNodes( - condition=IfCondition(use_composition), - target_container=container_name, - composable_node_descriptions=[ - ComposableNode( - package="moveit_servo", - plugin="moveit_servo::ServoNode", - name="servo_node", - parameters=parameters, - ), - ComposableNode( - package="robot_state_publisher", - plugin="robot_state_publisher::RobotStatePublisher", - name="robot_state_publisher", - parameters=parameters, - ), - ], ) # Add actions to launch description ld.add_action(control_node) - ld.add_action(load_nodes) - ld.add_action(container) - ld.add_action(load_composable_nodes) + ld.add_action(servo_node) return ld def generate_launch_description() -> LaunchDescription: + urdf_pkg = get_package_share_directory("rover_urdf") + urdf_path = os.path.join( + urdf_pkg, + "urdf", + "rover_urdf.urdf.xacro", + ) moveit_config = ( - MoveItConfigsBuilder("arm_urdf", package_name="arm_srdf") - .robot_description(file_path="config/arm_urdf.urdf.xacro") + MoveItConfigsBuilder("rover_urdf", package_name="arm_control") + .robot_description(file_path=urdf_path) .to_moveit_configs() ) return arm_launch(moveit_config) diff --git a/src/Arm/arm_control/launch/spawn_controllers.launch.py b/src/Arm/arm_control/launch/spawn_controllers.launch.py new file mode 100644 index 00000000..d556b8d7 --- /dev/null +++ b/src/Arm/arm_control/launch/spawn_controllers.launch.py @@ -0,0 +1,9 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + "rover_urdf", package_name="arm_control" + ).to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/src/Arm/arm_srdf/package.xml b/src/Arm/arm_control/package.xml similarity index 73% rename from src/Arm/arm_srdf/package.xml rename to src/Arm/arm_control/package.xml index 8e944e85..86da5870 100644 --- a/src/Arm/arm_srdf/package.xml +++ b/src/Arm/arm_control/package.xml @@ -1,12 +1,12 @@ - arm_srdf + arm_control 0.3.0 - An automatically generated package with all the configuration and launch files for using the arm_urdf with the MoveIt Motion Planning Framework + An automatically generated package with all the configuration and launch files for using the rover_urdf with the MoveIt Motion Planning Framework - William Fan + Connor Needham BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - William Fan + Connor Needham ament_cmake @@ -30,21 +30,15 @@ We don't include them by default to prevent installing gazebo and all its dependencies. --> - arm_urdf controller_manager moveit_configs_utils moveit_ros_move_group moveit_ros_visualization - moveit_ros_warehouse - moveit_servo - moveit_setup_assistant - robot_state_publisher + rover_urdf rviz2 rviz_common rviz_default_plugins - tf2_ros - warehouse_ros - ros_phoenix + xacro diff --git a/src/Arm/arm_srdf/.setup_assistant b/src/Arm/arm_srdf/.setup_assistant deleted file mode 100644 index 53331c5c..00000000 --- a/src/Arm/arm_srdf/.setup_assistant +++ /dev/null @@ -1,27 +0,0 @@ -moveit_setup_assistant_config: - urdf: - package: arm_urdf - relative_path: urdf/arm_urdf.urdf - srdf: - relative_path: config/arm_urdf.srdf - package_settings: - author_name: William Fan - author_email: wyf132@gmail.com - generated_timestamp: 1745543575 - control_xacro: - command: - - position - - velocity - state: - - position - - velocity - modified_urdf: - xacros: - - control_xacro - control_xacro: - command: - - position - - velocity - state: - - position - - velocity \ No newline at end of file diff --git a/src/Arm/arm_srdf/config/arm_urdf.srdf b/src/Arm/arm_srdf/config/arm_urdf.srdf deleted file mode 100644 index 0cf40133..00000000 --- a/src/Arm/arm_srdf/config/arm_urdf.srdf +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/Arm/arm_srdf/config/arm_urdf.urdf.xacro b/src/Arm/arm_srdf/config/arm_urdf.urdf.xacro deleted file mode 100644 index faa3be1f..00000000 --- a/src/Arm/arm_srdf/config/arm_urdf.urdf.xacro +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/src/Arm/arm_srdf/launch/setup_assistant.launch.py b/src/Arm/arm_srdf/launch/setup_assistant.launch.py deleted file mode 100644 index 6b28f796..00000000 --- a/src/Arm/arm_srdf/launch/setup_assistant.launch.py +++ /dev/null @@ -1,9 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_setup_assistant_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder( - "arm_urdf", package_name="arm_srdf" - ).to_moveit_configs() - return generate_setup_assistant_launch(moveit_config) diff --git a/src/Arm/arm_srdf/launch/talon.launch.py b/src/Arm/arm_srdf/launch/talon.launch.py deleted file mode 100644 index 1de194e4..00000000 --- a/src/Arm/arm_srdf/launch/talon.launch.py +++ /dev/null @@ -1,138 +0,0 @@ -# This launch file is deprecated. Use only for testing/debugging. - -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from math import pi - - -PUBLISH_PERIOD = 1 - - -def generate_launch_description(): - """Generate launch description with multiple components.""" - - container = ComposableNodeContainer( - name="PhoenixContainerArm", - namespace="", - package="ros_phoenix", - executable="phoenix_container", - parameters=[{"interface": "can0"}], - composable_node_descriptions=[ - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="base", - parameters=[ - {"id": 10}, - {"P": 12.0}, - {"I": 0.0}, - {"D": 0.0}, - {"sensor_multiplier": (2 * pi / 4096)}, - {"sensor_offset": 0.0}, - {"input_type": 2}, - {"max_voltage": 6.0}, - {"period_ms": PUBLISH_PERIOD}, - {"non_continuous": True}, - {"watchdog_ms": 300}, - ], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="act1", - parameters=[ - {"id": 11}, - {"P": 70.0}, - {"I": 0.0}, - {"D": 0.5}, - {"input_type": 2}, - {"invert_sensor": True}, - {"sensor_multiplier": (2 * pi / 4096)}, - {"sensor_offset": -3.14}, - {"max_voltage": 22.0}, - {"period_ms": PUBLISH_PERIOD}, - {"watchdog_ms": 300}, - {"non_continuous": True}, - ], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="act2", - parameters=[ - {"id": 14}, - {"P": 100.0}, - {"I": 0.0}, - {"D": 3.0}, - {"input_type": 2}, - {"invert": True}, - {"sensor_multiplier": (2 * pi / 4096)}, - {"sensor_offset": -2.02401481}, - {"max_voltage": 22.0}, - {"period_ms": PUBLISH_PERIOD}, - {"watchdog_ms": 300}, - {"non_continuous": True}, - ], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="elbow", - parameters=[ - {"id": 15}, - {"P": 4.0}, - {"I": 0.0}, - {"D": 0.0}, - {"max_voltage": 22.0}, - {"sensor_multiplier": (2 * pi / 4096)}, - {"sensor_offset": 0.826862284}, - {"input_type": 2}, - {"watchdog_ms": 300}, - {"period_ms": PUBLISH_PERIOD}, - ], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="wristTilt", - parameters=[ - {"id": 12}, - {"P": 20.0}, - {"I": 0.0}, - {"D": 0.0}, - {"max_voltage": 22.0}, - {"sensor_multiplier": (2 * pi / 4096)}, - {"sensor_offset": 4.0}, - {"input_type": 2}, - {"invert_sensor": True}, - {"invert": True}, - {"period_ms": PUBLISH_PERIOD}, - {"watchdog_ms": 300}, - {"non_continuous": True}, - ], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="wristTurn", - parameters=[ - {"id": 13}, - {"P": 0.008}, - {"I": 0.0}, - {"D": 0.0}, - {"invert": True}, - {"invert_sensor": True}, - { - "sensor_multiplier": 0.00000315420949 - }, # 2pi/(WRISTTURN_GEARBOX * WRISTTURN_GEAR) - {"period_ms": PUBLISH_PERIOD}, - {"watchdog_ms": 300}, - {"max_voltage": 22.0}, - ], - ), - ], - output="screen", - ) - - return launch.LaunchDescription([container]) diff --git a/src/Arm/arm_urdf/meshes/collision/Link_1.STL b/src/Arm/arm_urdf/meshes/collision/Link_1.STL deleted file mode 100644 index c081cd85..00000000 Binary files a/src/Arm/arm_urdf/meshes/collision/Link_1.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/collision/Link_2.STL b/src/Arm/arm_urdf/meshes/collision/Link_2.STL deleted file mode 100644 index 36945747..00000000 Binary files a/src/Arm/arm_urdf/meshes/collision/Link_2.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/collision/Link_3.STL b/src/Arm/arm_urdf/meshes/collision/Link_3.STL deleted file mode 100644 index 03d5f6e5..00000000 Binary files a/src/Arm/arm_urdf/meshes/collision/Link_3.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/collision/Link_4.STL b/src/Arm/arm_urdf/meshes/collision/Link_4.STL deleted file mode 100644 index b16a5232..00000000 Binary files a/src/Arm/arm_urdf/meshes/collision/Link_4.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/collision/Link_5.STL b/src/Arm/arm_urdf/meshes/collision/Link_5.STL deleted file mode 100644 index eb9566ba..00000000 Binary files a/src/Arm/arm_urdf/meshes/collision/Link_5.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/collision/Link_6.STL b/src/Arm/arm_urdf/meshes/collision/Link_6.STL deleted file mode 100644 index 88e57bd6..00000000 Binary files a/src/Arm/arm_urdf/meshes/collision/Link_6.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/collision/Link_7.STL b/src/Arm/arm_urdf/meshes/collision/Link_7.STL deleted file mode 100644 index 3feff962..00000000 Binary files a/src/Arm/arm_urdf/meshes/collision/Link_7.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/visual/Link_1.STL b/src/Arm/arm_urdf/meshes/visual/Link_1.STL deleted file mode 100644 index c081cd85..00000000 Binary files a/src/Arm/arm_urdf/meshes/visual/Link_1.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/visual/Link_2.STL b/src/Arm/arm_urdf/meshes/visual/Link_2.STL deleted file mode 100644 index 36945747..00000000 Binary files a/src/Arm/arm_urdf/meshes/visual/Link_2.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/visual/Link_3.STL b/src/Arm/arm_urdf/meshes/visual/Link_3.STL deleted file mode 100644 index 03d5f6e5..00000000 Binary files a/src/Arm/arm_urdf/meshes/visual/Link_3.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/visual/Link_4.STL b/src/Arm/arm_urdf/meshes/visual/Link_4.STL deleted file mode 100644 index b16a5232..00000000 Binary files a/src/Arm/arm_urdf/meshes/visual/Link_4.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/visual/Link_5.STL b/src/Arm/arm_urdf/meshes/visual/Link_5.STL deleted file mode 100644 index eb9566ba..00000000 Binary files a/src/Arm/arm_urdf/meshes/visual/Link_5.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/visual/Link_6.STL b/src/Arm/arm_urdf/meshes/visual/Link_6.STL deleted file mode 100644 index 88e57bd6..00000000 Binary files a/src/Arm/arm_urdf/meshes/visual/Link_6.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/meshes/visual/Link_7.STL b/src/Arm/arm_urdf/meshes/visual/Link_7.STL deleted file mode 100644 index 3feff962..00000000 Binary files a/src/Arm/arm_urdf/meshes/visual/Link_7.STL and /dev/null differ diff --git a/src/Arm/arm_urdf/setup.cfg b/src/Arm/arm_urdf/setup.cfg deleted file mode 100644 index 4276a1a2..00000000 --- a/src/Arm/arm_urdf/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/arm_urdf -[install] -install_scripts=$base/lib/arm_urdf diff --git a/src/Arm/arm_urdf/urdf/arm_urdf.urdf b/src/Arm/arm_urdf/urdf/arm_urdf.urdf deleted file mode 100644 index cc92e554..00000000 --- a/src/Arm/arm_urdf/urdf/arm_urdf.urdf +++ /dev/null @@ -1,383 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/Arm/arm_urdf/launch/launch.py b/src/URDF/rover_urdf/launch/launch.py similarity index 77% rename from src/Arm/arm_urdf/launch/launch.py rename to src/URDF/rover_urdf/launch/launch.py index 34fcf18c..e0e405db 100644 --- a/src/Arm/arm_urdf/launch/launch.py +++ b/src/URDF/rover_urdf/launch/launch.py @@ -7,13 +7,14 @@ DeclareLaunchArgument, ) from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, Command from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): # Get the launch directory - bringup_dir = get_package_share_directory("arm_urdf") + bringup_dir = get_package_share_directory("rover_urdf") # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration("rviz_config_file") @@ -34,27 +35,31 @@ def generate_launch_description(): ) declare_use_joint_state_pub_cmd = DeclareLaunchArgument( "use_joint_state_pub", - default_value="True", + default_value="False", description="Whether to start the joint state publisher", ) declare_use_rviz_cmd = DeclareLaunchArgument( - "use_rviz", default_value="True", description="Whether to start RVIZ" + "use_rviz", default_value="False", description="Whether to start RVIZ" ) declare_urdf_cmd = DeclareLaunchArgument( "urdf_file", - default_value=os.path.join(bringup_dir, "urdf", "arm_urdf.urdf"), + default_value=os.path.join(bringup_dir, "urdf", "rover_urdf.urdf.xacro"), description="Whether to start RVIZ", ) - start_robot_state_publisher_cmd = Node( condition=IfCondition(use_robot_state_pub), package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", output="screen", - # parameters=[{'use_sim_time': use_sim_time}], - arguments=[urdf_file], + parameters=[ + { + "robot_description": ParameterValue( + Command(["ros2 run xacro xacro ", urdf_file]), value_type=str + ) + } + ], ) start_joint_state_publisher_cmd = Node( @@ -63,7 +68,13 @@ def generate_launch_description(): executable="joint_state_publisher_gui", name="joint_state_publisher_gui", output="screen", - arguments=[urdf_file], + parameters=[ + { + "robot_description": ParameterValue( + Command(["ros2 run xacro xacro ", urdf_file]), value_type=str + ) + } + ], ) rviz_cmd = Node( diff --git a/src/URDF/rover_urdf/meshes/Antenna.STL b/src/URDF/rover_urdf/meshes/Antenna.STL new file mode 100644 index 00000000..e25504de Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Antenna.STL differ diff --git a/src/URDF/rover_urdf/meshes/EndEffector.STL b/src/URDF/rover_urdf/meshes/EndEffector.STL new file mode 100644 index 00000000..74567a11 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/EndEffector.STL differ diff --git a/src/URDF/rover_urdf/meshes/Frame.STL b/src/URDF/rover_urdf/meshes/Frame.STL new file mode 100644 index 00000000..98666a6c Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Frame.STL differ diff --git a/src/URDF/rover_urdf/meshes/GPS.STL b/src/URDF/rover_urdf/meshes/GPS.STL new file mode 100644 index 00000000..778c1c45 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/GPS.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_1.STL b/src/URDF/rover_urdf/meshes/Link_1.STL new file mode 100644 index 00000000..4b5ae0a9 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Link_1.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_2.STL b/src/URDF/rover_urdf/meshes/Link_2.STL new file mode 100644 index 00000000..c6e48c62 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Link_2.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_3.STL b/src/URDF/rover_urdf/meshes/Link_3.STL new file mode 100644 index 00000000..34910815 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Link_3.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_4.STL b/src/URDF/rover_urdf/meshes/Link_4.STL new file mode 100644 index 00000000..5013c976 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Link_4.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_5.STL b/src/URDF/rover_urdf/meshes/Link_5.STL new file mode 100644 index 00000000..874bb55e Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Link_5.STL differ diff --git a/src/URDF/rover_urdf/meshes/Link_6.STL b/src/URDF/rover_urdf/meshes/Link_6.STL new file mode 100644 index 00000000..66a87cb6 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Link_6.STL differ diff --git a/src/URDF/rover_urdf/meshes/Rocker_Left.STL b/src/URDF/rover_urdf/meshes/Rocker_Left.STL new file mode 100644 index 00000000..7f08d5da Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Rocker_Left.STL differ diff --git a/src/URDF/rover_urdf/meshes/Rocker_Right.STL b/src/URDF/rover_urdf/meshes/Rocker_Right.STL new file mode 100644 index 00000000..1ee9a335 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Rocker_Right.STL differ diff --git a/src/URDF/rover_urdf/meshes/Sus_Arm_Back_Left.STL b/src/URDF/rover_urdf/meshes/Sus_Arm_Back_Left.STL new file mode 100644 index 00000000..02fa253b Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Sus_Arm_Back_Left.STL differ diff --git a/src/URDF/rover_urdf/meshes/Sus_Arm_Back_Right.STL b/src/URDF/rover_urdf/meshes/Sus_Arm_Back_Right.STL new file mode 100644 index 00000000..915e5744 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Sus_Arm_Back_Right.STL differ diff --git a/src/URDF/rover_urdf/meshes/Sus_Arm_Front_Left.STL b/src/URDF/rover_urdf/meshes/Sus_Arm_Front_Left.STL new file mode 100644 index 00000000..623a2e5c Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Sus_Arm_Front_Left.STL differ diff --git a/src/URDF/rover_urdf/meshes/Sus_Arm_Front_Right.STL b/src/URDF/rover_urdf/meshes/Sus_Arm_Front_Right.STL new file mode 100644 index 00000000..47bb3633 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Sus_Arm_Front_Right.STL differ diff --git a/src/URDF/rover_urdf/meshes/Wheel_Arm_Back_Left.STL b/src/URDF/rover_urdf/meshes/Wheel_Arm_Back_Left.STL new file mode 100644 index 00000000..a59f6082 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Wheel_Arm_Back_Left.STL differ diff --git a/src/URDF/rover_urdf/meshes/Wheel_Arm_Back_Right.STL b/src/URDF/rover_urdf/meshes/Wheel_Arm_Back_Right.STL new file mode 100644 index 00000000..8fc94434 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Wheel_Arm_Back_Right.STL differ diff --git a/src/URDF/rover_urdf/meshes/Wheel_Arm_Front_Left.STL b/src/URDF/rover_urdf/meshes/Wheel_Arm_Front_Left.STL new file mode 100644 index 00000000..1ece0333 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Wheel_Arm_Front_Left.STL differ diff --git a/src/URDF/rover_urdf/meshes/Wheel_Arm_Front_Right.STL b/src/URDF/rover_urdf/meshes/Wheel_Arm_Front_Right.STL new file mode 100644 index 00000000..17764bb3 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Wheel_Arm_Front_Right.STL differ diff --git a/src/URDF/rover_urdf/meshes/Wheel_Left.STL b/src/URDF/rover_urdf/meshes/Wheel_Left.STL new file mode 100644 index 00000000..fc965311 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Wheel_Left.STL differ diff --git a/src/URDF/rover_urdf/meshes/Wheel_Right.STL b/src/URDF/rover_urdf/meshes/Wheel_Right.STL new file mode 100644 index 00000000..98cf2aaa Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Wheel_Right.STL differ diff --git a/src/URDF/rover_urdf/meshes/Zed.STL b/src/URDF/rover_urdf/meshes/Zed.STL new file mode 100644 index 00000000..d89ad800 Binary files /dev/null and b/src/URDF/rover_urdf/meshes/Zed.STL differ diff --git a/src/Arm/arm_urdf/package.xml b/src/URDF/rover_urdf/package.xml similarity index 55% rename from src/Arm/arm_urdf/package.xml rename to src/URDF/rover_urdf/package.xml index a43e0ade..c87d3a16 100644 --- a/src/Arm/arm_urdf/package.xml +++ b/src/URDF/rover_urdf/package.xml @@ -1,10 +1,10 @@ - arm_urdf + rover_urdf 0.0.0 - TODO: Package description - ros-industrial + Rover URDF - virtual double of the real life rover with arm + CPRT TODO: License declaration ament_copyright @@ -12,11 +12,11 @@ ament_pep257 python3-pytest - urdf - launch_ros - launch_ros - robot_state_publisher - joint_state_publisher_gui + urdf + launch_ros + launch_ros + robot_state_publisher + joint_state_publisher_gui ament_python diff --git a/src/Arm/arm_urdf/arm_urdf/__init__.py b/src/URDF/rover_urdf/resource/rover_urdf similarity index 100% rename from src/Arm/arm_urdf/arm_urdf/__init__.py rename to src/URDF/rover_urdf/resource/rover_urdf diff --git a/src/Arm/arm_urdf/resource/arm_urdf b/src/URDF/rover_urdf/rover_urdf/__init__.py similarity index 100% rename from src/Arm/arm_urdf/resource/arm_urdf rename to src/URDF/rover_urdf/rover_urdf/__init__.py diff --git a/src/URDF/rover_urdf/setup.cfg b/src/URDF/rover_urdf/setup.cfg new file mode 100644 index 00000000..371f69f3 --- /dev/null +++ b/src/URDF/rover_urdf/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rover_urdf +[install] +install_scripts=$base/lib/rover_urdf diff --git a/src/Arm/arm_urdf/setup.py b/src/URDF/rover_urdf/setup.py similarity index 80% rename from src/Arm/arm_urdf/setup.py rename to src/URDF/rover_urdf/setup.py index 247946c0..dd63ad33 100644 --- a/src/Arm/arm_urdf/setup.py +++ b/src/URDF/rover_urdf/setup.py @@ -1,7 +1,7 @@ from setuptools import setup from glob import glob -package_name = "arm_urdf" +package_name = "rover_urdf" setup( name=package_name, @@ -13,8 +13,7 @@ ("share/" + package_name, glob("launch/*.py")), ("share/" + package_name + "/urdf/", glob("urdf/*")), ("share/" + package_name + "/rviz/", glob("rviz/*")), - ("share/" + package_name + "/meshes/collision/", glob("meshes/collision/*")), - ("share/" + package_name + "/meshes/visual/", glob("meshes/visual/*")), + ("share/" + package_name + "/meshes/", glob("meshes/*")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/src/Arm/arm_urdf/test/test_copyright.py b/src/URDF/rover_urdf/test/test_copyright.py similarity index 100% rename from src/Arm/arm_urdf/test/test_copyright.py rename to src/URDF/rover_urdf/test/test_copyright.py diff --git a/src/Arm/arm_urdf/test/test_flake8.py b/src/URDF/rover_urdf/test/test_flake8.py similarity index 100% rename from src/Arm/arm_urdf/test/test_flake8.py rename to src/URDF/rover_urdf/test/test_flake8.py diff --git a/src/Arm/arm_urdf/test/test_pep257.py b/src/URDF/rover_urdf/test/test_pep257.py similarity index 100% rename from src/Arm/arm_urdf/test/test_pep257.py rename to src/URDF/rover_urdf/test/test_pep257.py diff --git a/src/Arm/arm_srdf/config/arm_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro similarity index 91% rename from src/Arm/arm_srdf/config/arm_urdf.ros2_control.xacro rename to src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro index f12d84ee..01d60b56 100644 --- a/src/Arm/arm_srdf/config/arm_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro @@ -4,8 +4,8 @@ - - ros2_control_rover_arm/RoverArmHardwareInterface + mock_components/GenericSystem + TalonSRX @@ -20,7 +20,7 @@ true false true - + @@ -37,7 +37,7 @@ true false true - + @@ -54,7 +54,7 @@ true true false - + @@ -71,7 +71,7 @@ true false false - + @@ -88,7 +88,7 @@ true true true - + @@ -105,7 +105,7 @@ true true true - + diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.xacro new file mode 100644 index 00000000..4b3a6be3 --- /dev/null +++ b/src/URDF/rover_urdf/urdf/arm_urdf.xacro @@ -0,0 +1,501 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/URDF/rover_urdf/urdf/chassis_urdf.xacro b/src/URDF/rover_urdf/urdf/chassis_urdf.xacro new file mode 100644 index 00000000..e407e446 --- /dev/null +++ b/src/URDF/rover_urdf/urdf/chassis_urdf.xacro @@ -0,0 +1,1127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/URDF/rover_urdf/urdf/rover_urdf.urdf.xacro b/src/URDF/rover_urdf/urdf/rover_urdf.urdf.xacro new file mode 100644 index 00000000..99d2947d --- /dev/null +++ b/src/URDF/rover_urdf/urdf/rover_urdf.urdf.xacro @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +