From 082efea9db66391e822ac687844816b5fb44a841 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Tue, 8 Nov 2022 19:41:55 +0200
Subject: [PATCH] Implement basic pen position logger

---
 src/draw_svg/CMakeLists.txt            |  1 +
 src/draw_svg/src/py/drawing_surface.py | 56 ++++++++++++++++++++++++++
 src/draw_svg/urdf/xarm_pen.urdf.xacro  | 22 +++++++++-
 3 files changed, 77 insertions(+), 2 deletions(-)
 create mode 100644 src/draw_svg/src/py/drawing_surface.py

diff --git a/src/draw_svg/CMakeLists.txt b/src/draw_svg/CMakeLists.txt
index d9b423c..5a96960 100644
--- a/src/draw_svg/CMakeLists.txt
+++ b/src/draw_svg/CMakeLists.txt
@@ -33,6 +33,7 @@ set(SRC_PY_DIR src/py)
 install(PROGRAMS
     ${SRC_PY_DIR}/draw_svg.py
     ${SRC_PY_DIR}/follow.py
+    ${SRC_PY_DIR}/drawing_surface.py
     DESTINATION lib/${PROJECT_NAME}
 )
 install(PROGRAMS
diff --git a/src/draw_svg/src/py/drawing_surface.py b/src/draw_svg/src/py/drawing_surface.py
new file mode 100644
index 0000000..9decff4
--- /dev/null
+++ b/src/draw_svg/src/py/drawing_surface.py
@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo"""
+
+import rclpy
+from geometry_msgs.msg import Pose, PoseWithCovariance
+from nav_msgs.msg import Odometry
+from pymoveit2 import MoveIt2
+from robots import lite6
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+
+class LogPen(Node):
+    def __init__(self):
+
+        super().__init__("log_pen")
+
+        # Create callback group that allows execution of callbacks in parallel without restrictions
+        self._callback_group = ReentrantCallbackGroup()
+
+        self.create_subscription(
+            msg_type=Odometry,
+            topic="/pen_position",
+            callback=self.pen_position_callback,
+            qos_profile=QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT,
+                                   history=HistoryPolicy.KEEP_LAST,
+                                   depth=1),
+            callback_group=self._callback_group,
+        )
+        self.get_logger().info("Initialization successful.")
+
+    def pen_position_callback(self, msg: Odometry):
+        """
+        Log position of pen
+        """
+        p = msg.pose.pose.position
+        #self.get_logger().info("x:{}, y:{}, z:{}".format(p[0], p[1], p[2]))
+        self.get_logger().info("x:{} y:{} z:{}".format(p.x, p.y, p.z))
+
+def main(args=None):
+
+    rclpy.init(args=args)
+
+    log_pen = LogPen()
+
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(log_pen)
+    executor.spin()
+
+    rclpy.shutdown()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/src/draw_svg/urdf/xarm_pen.urdf.xacro b/src/draw_svg/urdf/xarm_pen.urdf.xacro
index 06c2dd8..0ed4faa 100644
--- a/src/draw_svg/urdf/xarm_pen.urdf.xacro
+++ b/src/draw_svg/urdf/xarm_pen.urdf.xacro
@@ -52,9 +52,9 @@
 
   <joint name="eef_joint" type="fixed">
       <parent link="link_eef"/>
-      <child link="test_link"/>
+      <child link="pen_link"/>
   </joint>
-  <link name="test_link">
+  <link name="pen_link">
     <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
@@ -72,6 +72,24 @@
     </collision>
   </link>
 
+  <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
+  <gazebo>
+    <plugin name="pen_position" filename="libgazebo_ros_p3d.so">
+        <!-- <alwaysOn>true</alwaysOn> -->
+      <ros>
+        <remapping>odom:=pen_position</remapping>
+      </ros>
+      <frame_name>world</frame_name>
+      <!-- <body_name>pen_link</body_name> -->
+      <body_name>link6</body_name>
+      <!--       topic name is always /odom -->
+      <!-- <topic_name>pen_position</topic_name> -->
+      <update_rate>0.1</update_rate>
+      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
+      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
+    </plugin>
+  </gazebo>
+
 </robot>
 <!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
      <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>