Add cameraD455 mesh file to iiwa_description resources

.gitignore

@@ -3,3 +3,6 @@ install/
 build/
 .vscode
 .idea
+
+
+**.FCBak

src/iiwa_config/config/moveit/iiwa7.srdf

@@ -20,6 +20,8 @@
         <joint name="joint7"/>
         <joint name="tools_joint"/>
         <joint name="tool"/>
+        <joint name="cameraHolder_patron"/>
+        <joint name="cameraHolder_cameraD455"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="home" group="iiwa_arm">
@@ -75,4 +77,23 @@
     <disable_collisions link1="link6" link2="link7" reason="Adjacent"/>
     <disable_collisions link1="link6" link2="patron" reason="Never"/>
     <disable_collisions link1="link7" link2="patron" reason="Adjacent"/>
+    <disable_collisions link1="link7" link2="camera_holder" reason="Adjacent"/>
+    <disable_collisions link1="link6" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="link5" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="link4" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="link3" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="link2" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="link1" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="base_link" link2="camera_holder" reason="Never"/>
+    <disable_collisions link1="camera_holder" link2="patron" reason="Adjacent"/>
+    <disable_collisions link1="camera_hand" link2="camera_holder" reason="Adjacent"/>
+    <disable_collisions link1="camera_hand" link2="patron" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="link7" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="link6" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="link5" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="link4" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="link3" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="link2" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="link1" reason="Never"/>
+    <disable_collisions link1="camera_hand" link2="base_link" reason="Never"/>
 </robot>

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -18,11 +18,29 @@
 
     <xacro:include filename="$(find iiwa_description)/urdf/tools/patron.xacro"/>
 
-
     <xacro:if value="$(arg simulate)">
 
         <webots>
             <plugin type="webots_ros2_control::Ros2Control"/>
+
+            <device reference="camera_rgb" type="Camera">
+                <ros>
+                    <topicName>/camera/color</topicName>
+                    <updateRate>30</updateRate>
+                    <alwaysOn>True</alwaysOn>
+                    <frameName>camera_optical_frame</frameName>
+                </ros>
+            </device>
+
+            <device reference="camera_depth" type="RangeFinder">
+                <ros>
+                    <topicName>/camera/depth</topicName>
+                    <updateRate>30</updateRate>
+                    <alwaysOn>True</alwaysOn>
+                    <frameName>camera_optical_frame</frameName>
+                </ros>
+            </device>
+
         </webots>
 
         <ros2_control name="iiwaGazeboControl" type="system">

src/iiwa_description/urdf/tools/patron.xacro

@@ -1,31 +1,146 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_patron">
 
+    <link name="camera_holder">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="0.09"/>
+            <inertia ixx="0.0" ixy="0.0" ixz="0.0" 
+                     iyy="0.0" iyz="0.0" izz="0.0"/>
+        </inertial>
+        <visual name="">
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <!-- <mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/tools/CameraHolder.dae" /> -->
+                <mesh filename="package://iiwa_description/resource/meshes/tools/CameraHolder.dae"/>
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0.0 0.0 0.015" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <cylinder radius="0.035" length="0.03"/>
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0.0 0.08 0.015" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="0.03 0.1 0.03"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <link name="camera_hand">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="0.0"/>
+            <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+        </inertial>
+        <visual>
+            <origin xyz="-0.06 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <!-- <mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/tools/cameraD455.dae" /> -->
+                <mesh filename="package://iiwa_description/resource/meshes/tools/cameraD455.dae"/>
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0.0 0.014 0.0125" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="0.12 0.025 0.025"/>
+            </geometry>
+        </collision>
+    </link>
+
     <link name="patron">
         <inertial>
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-            <mass value="0.69"/>
+            <mass value="0.03"/>
-            <inertia ixx="0.000483150" ixy="0.0" ixz="0.0" 
+            <inertia ixx="0.0" ixy="0.0" ixz="0.0" 
-                     iyy="0.000483150" iyz="0.0" izz="0.000241750"/>
+            iyy="0.0" iyz="0.0" izz="0.0"/>
         </inertial>
-        <visual>
+        <visual name="">
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
             <geometry>
+                <!-- <mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/tools/patron.dae" /> -->
                 <mesh filename="package://iiwa_description/resource/meshes/tools/patron.dae"/>
             </geometry>
         </visual>
         <collision>
-            <origin xyz="0.0 0.0 0.0354" rpy="0.0 0.0 0.0"/>
+            <origin xyz="0.0 0.0 0.027" rpy="0.0 0.0 0.0"/>
             <geometry>
-                <cylinder radius="0.0315" length="0.071"/>
+                <cylinder radius="0.021" length="0.055"/>
             </geometry>
         </collision>
     </link>
-    
+
     <joint name="tool" type="fixed">
         <parent link="link_ee"/>
-        <child link="patron"/>
+        <child link="camera_holder"/>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <origin xyz="0 0 0" rpy="0 0 1.5708"/>
     </joint>
 
+    <joint name="cameraHolder_patron" type="fixed">
+        <origin xyz="0.0 0.0 0.03" rpy="0.0 0.0 0.0"/>
+        <parent link="camera_holder"/>
+        <child link="patron"/>
+    </joint>
+
+    <joint name="cameraHolder_cameraD455" type="fixed">
+        <origin xyz="0.0 0.1 0.014" rpy="0.0 0.0 0.0"/>
+        <parent link="camera_holder"/>
+        <child link="camera_hand"/>
+    </joint>
+
+    <link name="camera_optical_frame"/>
+    <joint name="camera_hand_to_optical" type="fixed">
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 3.14159"/>
+        <parent link="camera_hand"/>
+        <child link="camera_optical_frame"/>
+    </joint>
+
+    <gazebo reference="camera_optical_frame">
+
+        <sensor type="camera" name="camera_rgb">
+            <camera>
+                <horizontal_fov>1.5708</horizontal_fov>
+                <image>
+                    <width>640</width>
+                    <height>480</height>
+                    <format>R8G8B8</format>
+                </image>
+                <clip>
+                    <near>0.1</near>
+                    <far>10.0</far>
+                </clip>
+                <noise>
+                    <type>gaussian</type>
+                    <stddev>0.005</stddev>
+                </noise>
+            </camera>
+        </sensor>
+
+        <sensor type="depth" name="camera_depth">
+            <camera>
+                <horizontal_fov>1.5184</horizontal_fov>
+                <image>
+                    <width>640</width>
+                    <height>480</height>
+                </image>
+                <clip>
+                    <near>0.52</near>
+                    <far>6.0</far>
+                </clip>
+            </camera>
+            <range>
+                <min>0.52</min>
+                <max>6.0</max>
+                <resolution>0.001</resolution>
+            </range>
+            <noise>
+                <type>gaussian</type>
+                <stddev>0.02</stddev>
+            </noise>
+        </sensor>
+
+    </gazebo> 
+
 </robot>