robot visualization availiable

UofA-SPEAR · Jan 22, 2025 · d275426 · d275426
1 parent 063abd4
commit d275426
Show file tree

Hide file tree

Showing 15 changed files with 956 additions and 0 deletions.
diff --git a/documentation/visualizing_urdf.md b/documentation/visualizing_urdf.md
@@ -0,0 +1,210 @@
+## Install ZED SDK and CLONE Ros2 Wrapper 
+place ros2 wrapper inside the rover/src directory
+
+replace this file contents: ```/Software_2025/rover/src/zed-ros2-wrapper/zed_wrapper/urdf/zed_macro.urdf.xacro```
+
+with this ```
+<?xml version="1.0"?>
+
+<!--
+// Copyright 2022 Stereolabs
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+-->
+
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:include filename="$(find zed_wrapper)/urdf/include/materials.urdf.xacro" />
+  <xacro:property name="M_PI"     value="3.1415926535897931" />
+
+  <!--
+    Parameters:
+      - name: the camera's name.  should match the parameter sent to the launch file for this camera
+      - model: the tye of camera, one of zed, zedm, zed2, zed2i
+  -->
+  <xacro:macro name="zed_camera" params="name=zed model=zed">
+    <xacro:if value="${model == 'zed'}">
+      <xacro:property name="baseline" value="0.12" />
+      <xacro:property name="height" value="0.03" />
+      <xacro:property name="bottom_slope" value="0.05" />
+      <xacro:property name="screw_offset_x" value="0.0" />
+      <xacro:property name="screw_offset_z" value="0.0" />
+      <xacro:property name="optical_offset_x" value="-0.01" />
+    </xacro:if>
+    <xacro:if value="${model == 'zedm'}">
+      <xacro:property name="baseline" value="0.06" />
+      <xacro:property name="height" value="0.0265" />
+      <xacro:property name="bottom_slope" value="0.0" /> 
+      <xacro:property name="screw_offset_x" value="0.0" />
+      <xacro:property name="screw_offset_z" value="0.0" />
+      <xacro:property name="optical_offset_x" value="0.0" />
+    </xacro:if>
+    <xacro:if value="${model == 'zed2'}">
+      <xacro:property name="baseline" value="0.12" />
+      <xacro:property name="height" value="0.03" />
+      <xacro:property name="bottom_slope" value="0.05" />
+      <xacro:property name="screw_offset_x" value="0.0" />
+      <xacro:property name="screw_offset_z" value="0.0" />
+      <xacro:property name="optical_offset_x" value="-0.01" />
+    </xacro:if>
+    <xacro:if value="${model == 'zed2i'}">
+      <xacro:property name="baseline" value="0.12" />
+      <xacro:property name="height" value="0.03" />
+      <xacro:property name="bottom_slope" value="0.0" />
+      <xacro:property name="screw_offset_x" value="-0.01" />
+      <xacro:property name="screw_offset_z" value="0.0" />
+      <xacro:property name="optical_offset_x" value="-0.01" />
+      <xacro:property name="mono" value="false" />
+    </xacro:if>
+    <xacro:if value="${model == 'zedx'}">
+      <xacro:property name="baseline" value="0.12" />
+      <xacro:property name="height" value="0.032" />
+      <xacro:property name="bottom_slope" value="0.0" />
+      <xacro:property name="screw_offset_x" value="0.0" />
+      <xacro:property name="screw_offset_z" value="-0.016" />
+      <xacro:property name="optical_offset_x" value="-0.01" />
+    </xacro:if>
+    <xacro:if value="${model == 'zedxm'}">
+      <xacro:property name="baseline" value="0.05" />
+      <xacro:property name="height" value="0.032" />
+      <xacro:property name="bottom_slope" value="0.0" />
+      <xacro:property name="screw_offset_x" value="0.0" />
+      <xacro:property name="screw_offset_z" value="-0.016" />
+      <xacro:property name="optical_offset_x" value="-0.01" />
+    </xacro:if>
+
+    <!-- Camera mounting point (the threaded screw hole in the bottom) -->
+    <link name="${name}_camera_link" />
+
+    <!-- Camera Center -->
+    <link name="${name}_camera_center">
+      <xacro:unless value="${model == 'virtual'}">
+        <visual>
+          <origin xyz="${screw_offset_x} 0 ${screw_offset_z}" rpy="0 0 0"/>
+          <geometry>
+<mesh filename="file://$(find zed_msgs)/meshes/${model}.stl"/>
+          </geometry>
+          <material name="${model}_mat" />
+        </visual>      
+        <collision>
+          <origin xyz="${screw_offset_x} 0 ${screw_offset_z}" rpy="0 0 0"/>
+          <geometry>
+<mesh filename="file://$(find zed_msgs)/meshes/${model}.stl"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+    </link>
+    <joint name="${name}_camera_center_joint" type="fixed">
+      <parent link="${name}_camera_link"/>
+      <child link="${name}_camera_center"/>
+      <origin xyz="0 0 ${height/2}" rpy="0 ${bottom_slope} 0" />
+    </joint>
+
+    <xacro:unless value="${mono}">
+    <!-- Left Camera -->
+       <link name="${name}_left_camera_frame">
+        <xacro:if value="${model == 'virtual'}">
+          <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+  <mesh filename="file://$(find zed_msgs)/meshes/${model}.stl"/>
+            </geometry>
+            <material name="${model}_mat" />
+          </visual>
+          <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+  <mesh filename="file://$(find zed_msgs)/meshes/${model}.stl"/>
+            </geometry>
+          </collision>
+        </xacro:if>
+      </link>
+      <joint name="${name}_left_camera_joint" type="fixed">
+        <parent link="${name}_camera_center"/>
+        <child link="${name}_left_camera_frame"/>
+        <origin xyz="${optical_offset_x} ${baseline/2} 0" rpy="0 0 0" />
+      </joint>
+
+      <link name="${name}_left_camera_optical_frame"/>
+      <joint name="${name}_left_camera_optical_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
+        <parent link="${name}_left_camera_frame"/>
+        <child link="${name}_left_camera_optical_frame"/>
+      </joint>
+
+      <!-- Right Camera -->
+      <link name="${name}_right_camera_frame">
+        <xacro:if value="${model == 'virtual'}">
+          <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+  <mesh filename="file://$(find zed_msgs)/meshes/${model}.stl"/>
+            </geometry>
+            <material name="${model}_mat" />
+          </visual>      
+          <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+  <mesh filename="file://$(find zed_msgs)/meshes/${model}.stl"/>
+            </geometry>
+          </collision>
+        </xacro:if>
+      </link>
+      <joint name="${name}_right_camera_joint" type="fixed">
+        <parent link="${name}_camera_center"/>
+        <child link="${name}_right_camera_frame"/>
+        <origin xyz="${optical_offset_x} -${baseline/2} 0" rpy="0 0 0" />
+      </joint>
+
+      <link name="${name}_right_camera_optical_frame"/>
+      <joint name="${name}_right_camera_optical_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
+        <parent link="${name}_right_camera_frame"/>
+        <child link="${name}_right_camera_optical_frame"/>
+      </joint>
+
+
+      <!-- ZED2 Sensors -->
+      <xacro:if value="${model == 'zed2'}">
+        <link name="${name}_mag_link" />
+        <joint name="${name}_mag_joint" type="fixed">
+          <parent link="${name}_camera_center"/>
+          <child link="${name}_mag_link"/>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+        </joint>
+
+        <link name="${name}_baro_link" />
+        <joint name="${name}_baro_joint" type="fixed">
+          <parent link="${name}_camera_center"/>
+          <child link="${name}_baro_link"/>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+        </joint>
+
+        <link name="${name}_temp_left_link" />
+        <joint name="${name}_temp_left_joint" type="fixed">
+          <parent link="${name}_left_camera_frame"/>
+          <child link="${name}_temp_left_link"/>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+        </joint>
+
+        <link name="${name}_temp_right_link" />
+        <joint name="${name}_temp_right_joint" type="fixed">
+          <parent link="${name}_right_camera_frame"/>
+          <child link="${name}_temp_right_link"/>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+        </joint>
+      </xacro:if>
+  </xacro:unless>
+
+  </xacro:macro>
+</robot>
+```
diff --git a/kipp_arm/src/teleop/launch/arm.launch.py b/kipp_arm/src/teleop/launch/arm.launch.py
@@ -0,0 +1,61 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.event_handlers import OnProcessStart
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_demo_launch
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+
+
+def generate_launch_description():
+
+    param_file = os.path.join(get_package_share_directory("moveit_spear"), "config", "moveit_servo_config.yaml")
+
+    joystick_conv = Node(
+        package='teleop',
+        executable='SPEAR_Arm_Node',
+    )
+
+    joy_input = Node(
+        package='teleop',
+        executable='Joystick_Input',
+    )
+
+    # Integrate the small launch file functionality
+    moveit_config = MoveItConfigsBuilder("SPEAR_Arm", package_name="moveit_spear").to_moveit_configs()
+    demo_launch = generate_demo_launch(moveit_config)  # This should handle RViz implicitly
+
+    servo_node = Node(
+        package="moveit_servo",
+        executable="servo_node_main",
+        name='arm_servo',
+        parameters=[
+            param_file,
+            moveit_config.robot_description_kinematics,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description
+        ],
+        arguments=['--ros-args', '--log-level', 'INFO'],
+        output='screen',
+    )
+
+
+    # Modified ExecuteProcess part
+    execute_process = ExecuteProcess(
+        cmd=['kipp_arm/src/teleop/launch/service.sh'],
+        output='screen'
+    )
+
+    # Initialize the LaunchDescription object
+    ld = LaunchDescription()
+    ld.add_action(joy_input)
+    ld.add_action(joystick_conv)
+    ld.add_action(demo_launch)  # Adding the demo launch, which sets up RViz
+    ld.add_action(servo_node)
+    ld.add_action(execute_process)  # Adding the execute process action
+
+    return ld
diff --git a/rover/ZED_SDK.run b/rover/ZED_SDK.run
diff --git a/rover/src/kipp_description/kipp_description/__init__.py b/rover/src/kipp_description/kipp_description/__init__.py
diff --git a/rover/src/kipp_description/launch/kipp_state.launch.py b/rover/src/kipp_description/launch/kipp_state.launch.py
@@ -0,0 +1,45 @@
+import launch
+from launch.substitutions import Command, LaunchConfiguration
+import launch_ros
+import os
+
+def generate_launch_description():
+    pkg_share = launch_ros.substitutions.FindPackageShare(package='kipp_description').find('kipp_description')
+    default_model_path = os.path.join(pkg_share, 'urdf/kipp.urdf.xacro')
+    default_rviz_config_path = os.path.join(pkg_share, 'rviz/kipp.rviz')
+
+    robot_state_publisher_node = launch_ros.actions.Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
+    )
+    joint_state_publisher_node = launch_ros.actions.Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        arguments=[default_model_path],
+    )
+    joint_state_publisher_gui_node = launch_ros.actions.Node(
+        package='joint_state_publisher_gui',
+        executable='joint_state_publisher_gui',
+        name='joint_state_publisher_gui',
+        condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
+    )
+    rviz_node = launch_ros.actions.Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+    )
+
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
+                                            description='Absolute path to robot urdf file'),
+        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
+                                            description='Absolute path to rviz config file'),
+        joint_state_publisher_node,
+        #joint_state_publisher_gui_node,
+        robot_state_publisher_node,
+        rviz_node
+    ])
diff --git a/rover/src/kipp_description/package.xml b/rover/src/kipp_description/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>kipp_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">spearua</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/rover/src/kipp_description/resource/kipp_description b/rover/src/kipp_description/resource/kipp_description