Merge pull request #1065 from ROBOTIS-GIT/feature-humble-example-inte…

…ractive-marker Improved Interactive Marker Control & Fixed Lint Issues
ROBOTIS-GIT · Feb 21, 2025 · 39ad7c3 · 39ad7c3
2 parents cb47672 + 0771580
commit 39ad7c3
Show file tree

Hide file tree

Showing 21 changed files with 228 additions and 14 deletions.
diff --git a/turtlebot3/CHANGELOG.rst b/turtlebot3/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package turtlebot3
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* Modified Interactive Marker
+* Contributors: Junyeong Jeong, Jeonggeun Lim
+
 2.2.3 (2025-02-19)
 ------------------
 * Modified Patrol server and client

diff --git a/turtlebot3/package.xml b/turtlebot3/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     ROS 2 packages for TurtleBot3
   </description>

diff --git a/turtlebot3_bringup/CHANGELOG.rst b/turtlebot3_bringup/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_bringup
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* None
+
 2.2.3 (2025-02-19)
 ------------------
 * None

diff --git a/turtlebot3_bringup/package.xml b/turtlebot3_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_bringup</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     ROS 2 launch scripts for starting the TurtleBot3
   </description>

diff --git a/turtlebot3_cartographer/CHANGELOG.rst b/turtlebot3_cartographer/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_cartographer
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* None
+
 2.2.3 (2025-02-19)
 ------------------
 * None

diff --git a/turtlebot3_cartographer/package.xml b/turtlebot3_cartographer/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_cartographer</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     ROS 2 launch scripts for cartographer
   </description>

diff --git a/turtlebot3_description/CHANGELOG.rst b/turtlebot3_description/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* None
+
 2.2.3 (2025-02-19)
 ------------------
 * None

diff --git a/turtlebot3_description/package.xml b/turtlebot3_description/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_description</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     3D models of the TurtleBot3 for simulation and visualization
   </description>

diff --git a/turtlebot3_example/CHANGELOG.rst b/turtlebot3_example/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package turtlebot3_example
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* Modified Interactive Marker
+* Contributors: Junyeong Jeong, Jeonggeun Lim
+
 2.2.3 (2025-02-19)
 ------------------
 * Modified Patrol server and client

diff --git a/turtlebot3_example/package.xml b/turtlebot3_example/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_example</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     This package provides four basic examples for TurtleBot3 (i.e., interactive marker, object detection, patrol and position control).
   </description>
@@ -15,6 +15,7 @@
   <author email="jhshim@robotis.com">Ryan Shim</author>
   <author email="kkjong@robotis.com">Gilbert</author>
   <author email="willson@robotis.com">Will Son</author>
+  <author email="junyeong4321@gmail.com">Junyeong Jeong</author>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>

diff --git a/turtlebot3_example/setup.py b/turtlebot3_example/setup.py
@@ -5,7 +5,7 @@
 
 setup(
     name=package_name,
-    version='2.2.3',
+    version='2.2.4',
     packages=find_packages(),
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
@@ -20,9 +20,9 @@
     ],
     install_requires=['setuptools', 'launch'],
     zip_safe=True,
-    author=['Jeonggeun Lim', 'Will Son', 'Ryan Shim', 'Gilbert'],
-    author_email=['ljg@robotis.com', 'willson@robotis.com', 'jhshim@robotis.com',
-                  'kkjong@robotis.com'],
+    author=['JunyeongJeong', 'Jeonggeun Lim', 'Will Son', 'Ryan Shim', 'Gilbert'],
+    author_email=['junyeong4321@gmail.com', 'ljg@robotis.com', 'willson@robotis.com',
+                  'jhshim@robotis.com', 'kkjong@robotis.com'],
     maintainer='Pyo',
     maintainer_email='pyo@robotis.com',
     keywords=['ROS', 'ROS2', 'examples', 'rclpy'],
@@ -38,6 +38,9 @@
     license='Apache License, Version 2.0',
     entry_points={
         'console_scripts': [
+            'turtlebot3_interactive_marker = '
+            'turtlebot3_example.turtlebot3_interactive_marker.'
+            'turtlebot3_interactive_marker:main',
             'turtlebot3_patrol_server = \
                 turtlebot3_example.turtlebot3_patrol.turtlebot3_patrol_server:main',
             'turtlebot3_patrol_client = \

diff --git a/turtlebot3_example/turtlebot3_example/turtlebot3_interactive_marker/__init__.py b/turtlebot3_example/turtlebot3_example/turtlebot3_interactive_marker/__init__.py
diff --git a/...example/turtlebot3_example/turtlebot3_interactive_marker/turtlebot3_interactive_marker.py b/...example/turtlebot3_example/turtlebot3_interactive_marker/turtlebot3_interactive_marker.py
@@ -0,0 +1,177 @@
+#!/usr/bin/env python
+#
+# Copyright 2018 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Junyeong Jeong, Jeonggeun Lim, Gilbert
+
+import math
+import sys
+
+from geometry_msgs.msg import Twist
+from interactive_markers import InteractiveMarkerServer
+from nav_msgs.msg import Odometry
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from tf_transformations import euler_from_quaternion
+from tf_transformations import quaternion_from_euler
+from visualization_msgs.msg import InteractiveMarker
+from visualization_msgs.msg import InteractiveMarkerControl
+
+
+class Turtlebot3InteractiveMarker(Node):
+
+    def __init__(self):
+        super().__init__('turtlebot3_interactive_marker')
+
+        print('TurtleBot3 Interactive Markers')
+        print('----------------------------------------------')
+        print('Move red arrows while clicking the arrows')
+        print('Rotate with the circular handles along Z-axis')
+        print('----------------------------------------------')
+
+        qos = QoSProfile(depth=10)
+
+        self.odom = Odometry()
+        self.goal_position = None
+        self.goal_orientation = None
+        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos)
+
+        self.odom_sub = self.create_subscription(
+            Odometry,
+            'odom',
+            self.odom_callback,
+            qos)
+
+        self.server = InteractiveMarkerServer(self, 'turtlebot3_interactive_marker')
+
+        self.move_marker = InteractiveMarker()
+        self.move_marker.header.frame_id = 'odom'
+        self.move_marker.name = 'turtlebot3_move_marker'
+
+        move_control = InteractiveMarkerControl()
+        move_control.name = 'move_x'
+        move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        move_control.always_visible = True
+        self.move_marker.controls.append(move_control)
+
+        self.server.insert(self.move_marker, feedback_callback=self.processMoveFeedback)
+
+        rotate_marker = InteractiveMarker()
+        rotate_marker.header.frame_id = 'base_link'
+        rotate_marker.name = 'turtlebot3_rotate_marker'
+
+        rotate_control = InteractiveMarkerControl()
+        rotate_control.name = 'rotate_z'
+        rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        rotate_control.orientation.w = 1.0
+        rotate_control.orientation.x = 0.0
+        rotate_control.orientation.y = 1.0
+        rotate_control.orientation.z = 0.0
+        rotate_marker.controls.append(rotate_control)
+
+        self.server.insert(rotate_marker, feedback_callback=self.processRotateFeedback)
+
+        self.server.applyChanges()
+
+        self.create_timer(0.1, self.publish_cmd_vel)
+
+    def odom_callback(self, msg):
+        self.odom = msg
+
+    def processMoveFeedback(self, feedback):
+        self.goal_position = feedback.pose.position
+        self.goal_orientation = None
+
+        self.update_move_marker_pose()
+
+    def processRotateFeedback(self, feedback):
+        self.goal_orientation = feedback.pose.orientation
+        self.goal_position = None
+
+    def get_yaw(self):
+        q = self.odom.pose.pose.orientation
+        euler = euler_from_quaternion([q.x, q.y, q.z, q.w])
+        return euler[2]
+
+    def update_move_marker_pose(self):
+        current_x = self.odom.pose.pose.position.x
+        current_y = self.odom.pose.pose.position.y
+        current_yaw = self.get_yaw()
+
+        new_quat = quaternion_from_euler(0, 0, current_yaw)
+
+        self.move_marker.pose.position.x = current_x
+        self.move_marker.pose.position.y = current_y
+        self.move_marker.pose.orientation.x = new_quat[0]
+        self.move_marker.pose.orientation.y = new_quat[1]
+        self.move_marker.pose.orientation.z = new_quat[2]
+        self.move_marker.pose.orientation.w = new_quat[3]
+
+        self.server.insert(self.move_marker)
+        self.server.applyChanges()
+
+    def publish_cmd_vel(self):
+        twist = Twist()
+
+        if self.goal_position is not None:
+            current_x = self.odom.pose.pose.position.x
+            current_y = self.odom.pose.pose.position.y
+            current_yaw = self.get_yaw()
+
+            goal_x = self.goal_position.x
+            goal_y = self.goal_position.y
+
+            dx = goal_x - current_x
+            dy = goal_y - current_y
+            distance = math.sqrt(dx**2 + dy**2)
+
+            if distance < 0.01:
+                self.goal_position = None
+            else:
+                forward_speed = dx * math.cos(current_yaw) + dy * math.sin(current_yaw)
+                lateral_speed = -dx * math.sin(current_yaw) + dy * math.cos(current_yaw)
+
+                twist.linear.x = max(-0.1, min(0.1, forward_speed))
+                twist.linear.y = max(-0.1, min(0.1, lateral_speed))
+
+        elif self.goal_orientation is not None:
+            current_yaw = self.get_yaw()
+
+            goal_q = self.goal_orientation
+            goal_euler = euler_from_quaternion([goal_q.x, goal_q.y, goal_q.z, goal_q.w])
+            target_yaw = goal_euler[2]
+
+            yaw_diff = math.atan2(
+                math.sin(target_yaw - current_yaw), math.cos(target_yaw - current_yaw)
+            )
+
+            if abs(yaw_diff) < 0.01:
+                self.goal_orientation = None
+            else:
+                twist.angular.z = max(-0.5, min(0.5, yaw_diff))
+
+        self.cmd_vel_pub.publish(twist)
+        self.update_move_marker_pose()
+
+
+def main(args=None):
+    rclpy.init(args=sys.argv)
+    turtlebot3_interactive_marker = Turtlebot3InteractiveMarker()
+    rclpy.spin(turtlebot3_interactive_marker)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/turtlebot3_example/turtlebot3_example/turtlebot3_patrol/turtlebot3_patrol_client.py b/turtlebot3_example/turtlebot3_example/turtlebot3_patrol/turtlebot3_patrol_client.py
@@ -16,7 +16,6 @@
 #
 # Authors: Jeonggeun Lim, Ryan Shim, Gilbert
 
-
 import rclpy
 from rclpy.action import ActionClient
 from rclpy.node import Node

diff --git a/turtlebot3_navigation2/CHANGELOG.rst b/turtlebot3_navigation2/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_navigation2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* None
+
 2.2.3 (2025-02-19)
 ------------------
 * None

diff --git a/turtlebot3_navigation2/package.xml b/turtlebot3_navigation2/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_navigation2</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     ROS 2 launch scripts for navigation2
   </description>

diff --git a/turtlebot3_node/CHANGELOG.rst b/turtlebot3_node/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_node
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* None
+
 2.2.3 (2025-02-19)
 ------------------
 * None

diff --git a/turtlebot3_node/package.xml b/turtlebot3_node/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_node</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     TurtleBot3 driver node that include diff drive controller, odometry and tf node
   </description>

diff --git a/turtlebot3_teleop/CHANGELOG.rst b/turtlebot3_teleop/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_teleop
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.4 (2025-02-21)
+------------------
+* None
+
 2.2.3 (2025-02-19)
 ------------------
 * None

diff --git a/turtlebot3_teleop/package.xml b/turtlebot3_teleop/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_teleop</name>
-  <version>2.2.3</version>
+  <version>2.2.4</version>
   <description>
     Teleoperation node using keyboard for TurtleBot3.
   </description>

diff --git a/turtlebot3_teleop/setup.py b/turtlebot3_teleop/setup.py
@@ -5,7 +5,7 @@
 
 setup(
     name=package_name,
-    version='2.2.3',
+    version='2.2.4',
     packages=find_packages(exclude=[]),
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),