Package Delivery Robotics Infrastructure update

CustomRobots/quadrotor/launch/quadrotor.launch.py

@@ -24,13 +24,15 @@ def launch_setup(context):
     Y = LaunchConfiguration("Y")
     gz_sensor = LaunchConfiguration("sensor")
     gz_namespace = LaunchConfiguration("namespace")
+    gz_gripper = LaunchConfiguration("gripper")
 
     package_dir = get_package_share_directory("custom_robots")
 
     nodes_to_start = []
 
     sensor = gz_sensor.perform(context)
     namespace = gz_namespace.perform(context)
+    gripper = gz_gripper.perform(context)
 
     bridge_yaml = os.path.join(package_dir, "params", f"quadrotor_{sensor}.yaml")
 
@@ -49,6 +51,7 @@ def launch_setup(context):
         mappings={
             "camera": "true" if sensor == "camera" else "false",
             "namespace": namespace,
+            "gripper": gripper,
         },
     ).toxml()
 
@@ -166,6 +169,7 @@ def generate_launch_description():
     declared_arguments.append(DeclareLaunchArgument("P", default_value="0"))
     declared_arguments.append(DeclareLaunchArgument("Y", default_value="0"))
     declared_arguments.append(DeclareLaunchArgument("sensor", default_value="camera"))
+    declared_arguments.append(DeclareLaunchArgument("gripper", default_value="false"))
     declared_arguments.append(
         DeclareLaunchArgument(
             "namespace", description="Namespace to use", default_value="drone0"

CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro

@@ -3,16 +3,51 @@
   xmlns:xacro="http://ros.org/wiki/xacro">
 
   <xacro:arg name="camera" default="true"/>
+  <xacro:arg name="gripper" default="false"/>
   <xacro:arg name="namespace" default="drone0"/>
   <xacro:property name="namespace" value="$(arg namespace)"/>
+  <xacro:property name="gripper" value="$(arg gripper)"/>
 
   <xacro:include filename="$(find custom_robots)/models/quadrotor/quadrotor_gz.urdf.xacro"/>
   <xacro:include filename="$(find custom_robots)/models/quadrotor/quadrotor_common.urdf.xacro"/>
 
-  <xacro:quadrotor namespace="${namespace}"/>
+  <xacro:quadrotor namespace="${namespace}" gripper="${gripper}"/>
 
   <xacro:if value="$(arg camera)">
     <xacro:camera xyz="0 0 0" rpy="0 0 0" mass="0.035" name="frontal_cam" parent="base_link" namespace="${namespace}"/>
     <xacro:camera xyz="0 0 0" rpy="0 1.57 0" mass="0.035" name="ventral_cam" parent="base_link" namespace="${namespace}"/>
   </xacro:if>
+<<<<<<< HEAD
+
+  <!-- Electromagnet: flat ring hugging the drone belly, with an open center
+       so the downward ventral camera is not blocked (package_delivery). -->
+  <xacro:if value="$(arg gripper)">
+    <link name="magnet">
+      <inertial>
+        <mass value="0.02"/>
+        <inertia ixx="2e-5" ixy="0" ixz="0" iyy="2e-5" iyz="0" izz="4e-5"/>
+      </inertial>
+      <xacro:magnet_ring_seg i="0" n="12" radius="0.06"/>
+    </link>
+    <joint name="magnet_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 -0.05"/>
+      <parent link="base_link"/>
+      <child link="magnet"/>
+    </joint>
+
+    <!-- Magnetic gripper: model plugin, so it belongs to this drone (works
+         with renames / several drones). The exercise publishes the graspable
+         list on the graspable topic. -->
+    <gazebo>
+      <plugin filename="libdrone_gripper.so" name="drone_gripper::DroneGripper">
+        <gripper_link>base_link</gripper_link>
+        <attach_distance>0.15</attach_distance>
+        <magnet_topic>/${namespace}/gripper/magnet</magnet_topic>
+        <state_topic>/${namespace}/gripper/attached</state_topic>
+        <graspable_topic>/${namespace}/gripper/graspable</graspable_topic>
+      </plugin>
+    </gazebo>
+  </xacro:if>
+=======
+>>>>>>> upstream/drone-amazon-delivery
 </robot>

CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro

@@ -10,6 +10,26 @@
     <color rgba="0 0 1 1.0"/>
   </material>
 
+  <material name="MagnetBlack">
+    <color rgba="0.05 0.05 0.05 1.0"/>
+  </material>
+
+  <!-- One box segment of the magnet ring. The ring leaves an open center
+       so it does not block the downward-facing ventral camera. -->
+  <xacro:macro name="magnet_ring_seg" params="i n radius">
+    <xacro:if value="${i &lt; n}">
+      <xacro:property name="ang" value="${2*pi*i/n}"/>
+      <visual name="magnet_seg_${i}">
+        <origin xyz="${radius*cos(ang)} ${radius*sin(ang)} 0" rpy="0 0 ${ang + pi/2}"/>
+        <geometry>
+          <box size="${2*radius*sin(pi/n)} 0.02 0.02"/>
+        </geometry>
+        <material name="MagnetBlack"/>
+      </visual>
+      <xacro:magnet_ring_seg i="${i+1}" n="${n}" radius="${radius}"/>
+    </xacro:if>
+  </xacro:macro>
+
   <xacro:macro name="rotor" params="rotor parent xyz color namespace">
     <link name='rotor_${rotor}'>
       <inertial>
@@ -42,7 +62,7 @@
     </joint>
   </xacro:macro>
 
-  <xacro:macro name="quadrotor" params="namespace">
+  <xacro:macro name="quadrotor" params="namespace gripper">
 
     <!-- This is from quadrotor_base from as2 -->
     <link name="base_link">
@@ -91,6 +111,23 @@
     <xacro:rotor rotor="2" parent="base_link" xyz="0.13 0.22 0.023" color="Blue" namespace="${namespace}"/>
     <xacro:rotor rotor="3" parent="base_link" xyz="-0.13 -0.2 0.023" color="Red" namespace="${namespace}"/>
 
+    <!-- Electromagnet: flat ring hugging the drone belly, with an open center
+        so the downward ventral camera is not blocked (package_delivery). -->
+    <xacro:if value="${gripper}">
+      <link name="magnet">
+        <inertial>
+          <mass value="0.02"/>
+          <inertia ixx="2e-5" ixy="0" ixz="0" iyy="2e-5" iyz="0" izz="4e-5"/>
+        </inertial>
+        <xacro:magnet_ring_seg i="0" n="12" radius="0.06"/>
+      </link>
+      <joint name="magnet_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 -0.05"/>
+        <parent link="base_link"/>
+        <child link="magnet"/>
+      </joint>
+    </xacro:if>
+
     <xacro:tf freq="100"/>
     <xacro:imu mass="0.005" parent="base_link" namespace="${namespace}"/>
     <xacro:odom topic="odom" freq="100" namespace="${namespace}"/>

Industrial/drone_gripper/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.8)
+project(drone_gripper)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+############################
+# Dependencies
+############################
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+find_package(gz-sim8 REQUIRED)
+find_package(gz-plugin2 REQUIRED)
+
+############################
+# Plugin library
+############################
+
+add_library(drone_gripper SHARED
+  src/drone_gripper.cpp
+)
+
+target_link_libraries(drone_gripper
+  gz-sim8::gz-sim8
+  gz-plugin2::gz-plugin2
+)
+
+ament_target_dependencies(drone_gripper
+  rclcpp
+  std_msgs
+)
+
+############################
+# Install plugin
+############################
+
+install(
+  TARGETS drone_gripper
+  LIBRARY DESTINATION lib
+)
+
+############################
+# Environment hook (.dsv)
+############################
+
+install(
+  FILES env-hooks/drone_gripper.dsv
+  DESTINATION share/${PROJECT_NAME}/environment
+)
+
+############################
+# Export
+############################
+
+ament_export_libraries(drone_gripper)
+
+ament_export_dependencies(
+  gz-sim8
+  gz-plugin2
+)
+
+ament_package()

Industrial/drone_gripper/env-hooks/drone_gripper.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib

Industrial/drone_gripper/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+
+  <name>drone_gripper</name>
+  <version>0.0.1</version>
+
+  <description>Gazebo Harmonic magnetic gripper system plugin for drones</description>
+
+  <maintainer email="dev@todo.com">dev</maintainer>
+
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+
+  <depend>std_msgs</depend>
+
+  <depend>gz-sim</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>