Merge pull request #43 from kzorina/hpp_test_code

Initial demo setup for HPP pick and place plan

Author: MaximilienNaveau

agimus_demo_04_visual_servoing/.exists

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.22.1)
+
+#
+# Project definition
+#
+project(agimus_demo_05_pick_and_place LANGUAGES CXX)
+
+#
+# Handle dependencies by reading the package.xml
+#
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+#
+# Build Python package.
+#
+ament_python_install_package(${PROJECT_NAME})
+install(PROGRAMS scripts/pick_and_place_node DESTINATION lib/${PROJECT_NAME})
+
+#
+# Unit tests
+#
+include(CTest)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_auto_find_test_dependencies()
+  # Integration test of the roscontrol controller with simulation on Talos.
+  # add_rostest(tests/test_integration.py)
+endif()
+
+#
+# Installation
+#
+ament_auto_package(INSTALL_TO_SHARE config launch urdf srdf)

agimus_demo_05_pick_and_place/CMakeLists.txt

@@ -0,0 +1,32 @@
+AGIMUS demo 05 pick and place
+-----------------------------
+
+The purpose of this demo is to use the AGIMUS architecture to unbox some objects.
+Expected behaviors:
+    - The robot goes over a box of object.
+    - The robot start approaching one object.
+    - The robot goes to grasp an object.
+    - The robot move the object above a desired location.
+    - The robot drops the object.
+
+## Install dependencies and build.
+
+```bash
+vcs import src < src/agimus-demos/agimus_demo_05_pick_and_place/dependencies.repos
+rosdep update --rosdistro $ROS_DISTRO
+rosdep install -y -i --from-paths src --rosdistro $ROS_DISTRO --skip-keys libfranka
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug --symlink-install
+source install/setup.bash
+```
+
+## Start the demo in simulation using the Panda robot.
+```bash
+cd workspace
+reset && source install/setup.bash && ros2 launch agimus_demo_05_pick_and_place bringup.launch.py use_gazebo:=true
+```
+After the xterm terminal is opened, type there `o.pick_and_place('obj_26')`.
+
+## Start the demo on hardware using the Panda robot.
+```bash
+ros2 launch agimus_demo_05_pick_and_place bringup_hw.launch.py arm_id:=fer robot_ip:=<fci-ip>
+```

agimus_demo_05_pick_and_place/README.md

@@ -0,0 +1,43 @@
+from copy import deepcopy
+
+import rclpy
+from rclpy.node import Node
+from rclpy.task import Future
+from rclpy.qos import qos_profile_system_default, QoSProfile
+from rclpy.qos_overriding_options import QoSOverridingOptions
+
+
+class AsyncSubscriber:
+    def __init__(
+        self,
+        node: Node,
+        message_type: type,
+        topic_name: str,
+        qos_profile: QoSProfile = qos_profile_system_default,
+    ) -> None:
+        self._node = node
+
+        # Joint states subscriber
+        self._subscription = self._node.create_subscription(
+            message_type,
+            topic_name,
+            self._message_cb,
+            qos_profile=qos_profile,
+            qos_overriding_options=QoSOverridingOptions.with_default_policies(),
+        )
+
+        self._future = Future()
+        # Initialize the future to be already in state `Done`
+        self._future.set_result(None)
+
+    def _message_cb(self, msg):
+        """Callback function to latest value of the message."""
+        # Complete the future if it was waiting
+        if not self._future.done():
+            self._future.set_result(msg)
+
+    def wait_for_future(self):
+        """Blocks until a new joint state is received."""
+        self._future = Future()  # Reset future for next call
+        rclpy.spin_until_future_complete(self._node, self._future)
+        return deepcopy(self._future.result())