Merge pull request #8 from alliander-opensource/feat/add_realsense_to…

…_xacro

Add xacro for franka with possibility to load realsense.

rcdt_franka/CMakeLists.txt

@@ -12,7 +12,7 @@ find_package(ament_cmake_python REQUIRED)
 # Install project files
 install(
   DIRECTORY
-  launch config
+  launch config urdf
   DESTINATION share/${PROJECT_NAME}
 )
 

rcdt_franka/launch/franka.launch.py

@@ -13,23 +13,22 @@
 )
 
 use_sim_arg = LaunchArgument("simulation", True, [True, False])
+use_rviz_arg = LaunchArgument("rviz", True, [True, False])
+use_realsense_arg = LaunchArgument("realsense", False, [True, False])
 moveit_mode_arg = LaunchArgument("moveit", "off", ["node", "rviz", "servo", "off"])
-use_rviz_arg = LaunchArgument("rviz", False, [True, False])
 
 
 def launch_setup(context: LaunchContext) -> None:
     use_sim = use_sim_arg.value(context)
-    moveit_mode = moveit_mode_arg.value(context)
     use_rviz = use_rviz_arg.value(context)
+    use_realsense = use_realsense_arg.value(context)
+    moveit_mode = moveit_mode_arg.value(context)
 
-    xacro_path = get_file_path(
-        "franka_description", ["robots", "fr3"], "fr3.urdf.xacro"
-    )
-    xacro_arguments = {"ros2_control": "true"}
-    if use_sim:
-        xacro_arguments["gazebo"] = "true"
-    else:
-        xacro_arguments["robot_ip"] = "172.16.0.2"
+    xacro_path = get_file_path("rcdt_franka", ["urdf"], "franka.urdf.xacro")
+    xacro_arguments = {}
+    xacro_arguments["robot_ip"] = "172.16.0.2"
+    xacro_arguments["gazebo"] = "true" if use_sim else "false"
+    xacro_arguments["load_realsense"] = "true" if use_realsense else "false"
     robot_description = get_robot_description(xacro_path, xacro_arguments)
 
     robot_state_publisher = Node(
@@ -133,6 +132,7 @@ def generate_launch_description() -> LaunchDescription:
         [
             use_sim_arg.declaration,
             use_rviz_arg.declaration,
+            use_realsense_arg.declaration,
             moveit_mode_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]

rcdt_franka/urdf/franka.urdf.xacro

@@ -0,0 +1,56 @@
+<?xml version='1.0' encoding='utf-8'?>
+<!--
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="franka">
+
+  <!-- Global Franka definitions: -->
+  <xacro:arg name="arm_id" default="fr3" />
+  <xacro:arg name="ee_id" default="franka_hand" />
+
+  <!-- Franka related: -->
+  <xacro:arg name="parent" default="world" />
+  <xacro:arg name="xyz" default="0 0 0" />
+  <xacro:arg name="gazebo" default="false" />
+  <xacro:arg name="ign_ros2_control" default="false" />
+  <xacro:arg name="robot_ip" default="" />
+
+  <!-- Realsense related: -->
+  <xacro:arg name="load_realsense" default="true" />
+  <xacro:arg name="pos_realsense" default="0.05 0 0.05" />
+  <xacro:arg name="parent_realsense" default="fr3_hand" />
+  <xacro:arg name="rot_realsense" default="0 ${-0.5*pi} 0" />
+
+  <!-- Load arm: -->
+  <xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro" />
+  <xacro:franka_robot
+    arm_id="$(arg arm_id)"
+    joint_limits="${xacro.load_yaml('$(find franka_description)/robots/$(arg arm_id)/joint_limits.yaml')}"
+    inertials="${xacro.load_yaml('$(find franka_description)/robots/$(arg arm_id)/inertials.yaml')}"
+    kinematics="${xacro.load_yaml('$(find franka_description)/robots/$(arg arm_id)/kinematics.yaml')}"
+    dynamics="${xacro.load_yaml('$(find franka_description)/robots/$(arg arm_id)/dynamics.yaml')}"
+    parent="$(arg parent)"
+    ee_id="$(arg ee_id)"
+    gazebo="$(arg gazebo)"
+    xyz="$(arg xyz)" />
+
+  <!-- Load ros2 control. gazebo to LOAD and USE plugin. ign_ros2_control to only USE plugin. -->
+  <xacro:include filename="$(find franka_description)/robots/common/franka_arm.ros2_control.xacro" />
+  <xacro:franka_arm_ros2_control
+    arm_id="$(arg arm_id)"
+    robot_ip="$(arg robot_ip)"
+    hand="true"
+    gazebo="$(arg gazebo)"
+    ign_ros2_control="$(arg ign_ros2_control)" />
+
+  <!-- Load realsense: -->
+  <xacro:if value="$(arg load_realsense)">
+    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+    <xacro:sensor_d435 parent="$(arg parent_realsense)">
+      <origin xyz="$(arg pos_realsense)" rpy="$(arg rot_realsense)" />
+    </xacro:sensor_d435>
+  </xacro:if>
+
+</robot>