Add dingo industrial robot

CustomRobots/CMakeLists.txt

@@ -95,6 +95,10 @@ install(
     quadrotor/params
     quadrotor/launch
 
+    dingo/model
+    dingo/params
+    dingo/launch
+
     rover_4wd/model
     rover_4wd/params
     rover_4wd/launch

CustomRobots/dingo/launch/dingo.launch.py

@@ -0,0 +1,113 @@
+import os
+import xacro
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, Command, IfElseSubstitution
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def launch_setup(context):
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    x = LaunchConfiguration("x")
+    y = LaunchConfiguration("y")
+    z = LaunchConfiguration("z")
+    R = LaunchConfiguration("R")
+    P = LaunchConfiguration("P")
+    Y = LaunchConfiguration("Y")
+    gz_noise = LaunchConfiguration("noise")
+
+    package_dir = get_package_share_directory("custom_robots")
+
+    nodes_to_start = []
+
+    bridge_yaml = os.path.join(package_dir, "params", f"dingo.yaml")
+
+    # =========================
+    # ROBOT DESCRIPTION (URDF)
+    # =========================
+    xacro_file = os.path.join(
+        package_dir,
+        "model",
+        "dingo",
+        "dingo.urdf.xacro",
+    )
+
+    robot_description_content = xacro.process_file(
+        xacro_file,
+        mappings={
+            "namespace": "do150",
+            "noise_level": gz_noise.perform(context),
+        },
+    ).toxml()
+
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[robot_description, {"use_sim_time": True}],
+    )
+
+    gz_spawn_entity = Node(
+        package="ros_gz_sim",
+        executable="create",
+        arguments=[
+            "-topic",
+            "/robot_description",
+            "-name",
+            "do150",
+            "-allow_renaming",
+            "true",
+            "-x",
+            x,
+            "-y",
+            y,
+            "-z",
+            z,
+            "-R",
+            R,
+            "-P",
+            P,
+            "-Y",
+            Y,
+        ],
+        output="screen",
+    )
+
+    gz_ros2_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["--ros-args", "-p", f"config_file:={bridge_yaml}"],
+        output="screen",
+    )
+
+    nodes_to_start.append(robot_state_publisher_node)
+    nodes_to_start.append(gz_spawn_entity)
+    nodes_to_start.append(gz_ros2_bridge)
+
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = []
+
+    # Add any entry parameter
+    declared_arguments.append(
+        DeclareLaunchArgument("use_sim_time", default_value="true")
+    )
+    declared_arguments.append(DeclareLaunchArgument("x", default_value="0"))
+    declared_arguments.append(DeclareLaunchArgument("y", default_value="0"))
+    declared_arguments.append(DeclareLaunchArgument("z", default_value="0"))
+    declared_arguments.append(DeclareLaunchArgument("R", default_value="0"))
+    declared_arguments.append(DeclareLaunchArgument("P", default_value="0"))
+    declared_arguments.append(DeclareLaunchArgument("Y", default_value="0"))
+    declared_arguments.append(DeclareLaunchArgument("noise", default_value="none"))
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

CustomRobots/dingo/model/dingo/attachments/top_plate.urdf.xacro

@@ -0,0 +1,161 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <material name="clearpath_dark_grey">
+    <color rgba="0.2 0.2 0.2 1.0" />
+  </material>
+  <material name="clearpath_medium_grey">
+    <color rgba="0.6 0.6 0.6 1.0" />
+  </material>
+  <material name="clearpath_light_grey">
+    <color rgba="0.8 0.8 0.8 1.0" />
+  </material>
+  <material name="clearpath_yellow">
+    <color rgba="0.8 0.8 0.0 1.0" />
+  </material>
+  <material name="clearpath_black">
+    <color rgba="0.15 0.15 0.15 1.0" />
+  </material>
+  <material name="clearpath_white">
+    <color rgba="1.0 1.0 1.0 1.0" />
+  </material>
+  <material name="clearpath_red">
+    <color rgba="1.0 0.0 0.0 1.0" />
+  </material>
+
+  <material name="clearpath_aluminum">
+    <color rgba="0.5 0.5 0.5 1"/>
+  </material>
+  <material name="clearpath_plastic">
+    <color rgba="0.1 0.1 0.1   1"/>
+  </material>
+
+  <xacro:property name="guard_include_zed" value="true" lazy_eval="true"/>
+
+
+  <!--
+        Standard Mount: adds link (w/o geometry) at given position
+        - assumes orientation of parent
+    -->
+  <xacro:macro name="mount" params="name parent_link coordinate:=None *origin">
+    <!-- Add Empty Mount -->
+    <xacro:if value="${coordinate != None}">
+      <link name="${name}_mount_${coordinate}"/>
+      <joint name="${name}_mount_${coordinate}_joint" type="fixed">
+        <child link="${name}_mount_${coordinate}"/>
+        <parent link="${parent_link}"/>
+        <xacro:insert_block name="origin"/>
+      </joint>
+    </xacro:if>
+    <xacro:if value="${coordinate == None}">
+      <link name="${name}_mount"/>
+      <joint name="${name}_mount_joint" type="fixed">
+        <child link="${name}_mount"/>
+        <parent link="${parent_link}"/>
+        <xacro:insert_block name="origin"/>
+      </joint>
+    </xacro:if>
+  </xacro:macro>
+
+  <!--
+    Partial Riser Mounts: adds standard mounts for the specified row at the specified level
+    - requires that base_mounts have been added
+    - level (i.e. height) is w.r.t. the base level
+    -->
+  <xacro:macro name="row_mounts" params="name parent_link row column columns x:=0 y:=0 z:=0 spacing:=0.08 iteration:=0">
+    <xacro:property name="chr" value="${dict(d97='a', d98='b', d99='c', d100='d', d101='e', d102='f', d103='g', d104='h', d105='i', d106='j', d107='k', d108='l', d109='m', d110='n', d111='o', d112='p', d113='q', d114='r', d115='s', d116='t', d117='u', d118='v', d119='w', d120='x', d121='y', d122='z')}" />
+    <xacro:if value="${columns > iteration}">
+      <xacro:property name="coordinate" value="${chr['d' + str(python.ord(column))] + python.str(row)}" />
+      <xacro:mount name="${name}" parent_link="${parent_link}" coordinate="${coordinate}">
+        <origin xyz="${x} ${y - spacing * iteration} ${z}"/>
+      </xacro:mount>
+      <xacro:row_mounts name="${name}" parent_link="${parent_link}" row="${row}" column="${chr['d' + str(python.ord(column) + 1)]}" columns="${columns}" spacing="${spacing}" x="${x}" y="${y}" z="${z}" iteration="${iteration + 1}" />
+    </xacro:if>
+  </xacro:macro>
+
+  <xacro:macro name="full_mounts" params="name parent_link row column rows columns x:=0 y:=0 z spacing:=0.08 iteration:=0">
+    <xacro:if value="${rows > iteration}">
+      <xacro:row_mounts name="${name}" parent_link="${parent_link}" row="${row}" column="${column}" columns="${columns}" spacing="${spacing}" x="${x - spacing * iteration}" y="${y}" z="${z}" />
+      <xacro:full_mounts name="${name}" parent_link="${parent_link}" row="${python.str(python.int(row) + 1)}" column="${column}" rows="${rows}" columns="${columns}" spacing="${spacing}" x="${x}" y="${y}" z="${z}" iteration="${iteration + 1}"/>
+    </xacro:if>
+  </xacro:macro>
+  <xacro:macro name="top_plate" params="name model:=pacs parent_link:=default_mount height:=0.2 *origin">
+    <!-- PACS Model -->
+    <!-- Variables -->
+    <xacro:property name="plate_x_size" value="0.510"/>
+    <xacro:property name="plate_y_size" value="0.430"/>
+    <xacro:property name="plate_z_size" value="0.00635"/>
+
+    <xacro:property name="post_radius" value="0.006"/>
+    <xacro:property name="post_x_offset" value="0.040"/>
+    <xacro:property name="post_y_offset" value="0.040"/>
+
+    <!-- Spawn Top Plate -->
+    <link name="${name}_link">
+      <!-- Top Plate Mesh Visual -->
+      <visual>
+        <origin xyz="0 0 ${height}"/>
+        <material name="clearpath_dark_grey" />
+        <geometry>
+          <mesh filename="file://$(find custom_robots)/model/dingo/meshes/pacs_top_plate.stl" />
+        </geometry>
+      </visual>
+      <!-- Top Plate Box Collision -->
+      <collision>
+        <origin xyz="0 0 ${height + plate_z_size/2}"/>
+        <material name="clearpath_dark_grey"/>
+        <geometry>
+          <box size="${plate_x_size} ${plate_y_size} ${plate_z_size}"/>
+        </geometry>
+      </collision>
+      <!-- Quad Post Visual -->
+      <visual>
+        <origin xyz="${post_x_offset} ${post_y_offset} ${height/2}"/>
+        <material name="clearpath_dark_grey"/>
+        <geometry>
+          <cylinder radius="${post_radius}" length="${height}"/>
+        </geometry>
+      </visual>
+      <visual>
+        <origin xyz="${post_x_offset} ${-post_y_offset} ${height/2}"/>
+        <material name="clearpath_dark_grey"/>
+        <geometry>
+          <cylinder radius="${post_radius}" length="${height}"/>
+        </geometry>
+      </visual>
+      <visual>
+        <origin xyz="${-post_x_offset} ${post_y_offset} ${height/2}"/>
+        <material name="clearpath_dark_grey"/>
+        <geometry>
+          <cylinder radius="${post_radius}" length="${height}"/>
+        </geometry>
+      </visual>
+      <visual>
+        <origin xyz="${-post_x_offset} ${-post_y_offset} ${height/2}"/>
+        <material name="clearpath_dark_grey"/>
+        <geometry>
+          <cylinder radius="${post_radius}" length="${height}"/>
+        </geometry>
+      </visual>
+      <!-- Quad Post Collision -->
+      <collision>
+        <origin xyz="0 0 ${height/2}"/>
+        <material name="clearpath_dark_grey"/>
+        <geometry>
+          <box size="${2*post_x_offset + post_radius} ${2*post_y_offset} ${height}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- Attach PACS Top Plate -->
+    <joint name="${name}_joint" type="fixed">
+      <child link="${name}_link"/>
+      <parent link="${parent_link}"/>
+      <xacro:insert_block name="origin"/>
+    </joint>
+
+    <!-- Generate PACS mounts -->
+    <xacro:full_mounts name="${name}" parent_link="${name}_link" column="a" row="1" columns="5" rows="6" x="0.20" y="0.16" z="${height + plate_z_size}" />
+
+  </xacro:macro>
+</robot>