upload content

2022-02-24 22:45:51 +01:00
parent f376898190
commit 3659e07efc
472 changed files with 53075 additions and 0 deletions
@@ -0,0 +1,153 @@
+from distutils.log import error
+from matplotlib.pyplot import twinx
+import rclpy
+from rclpy.node import Node
+import math
+
+from turtlesim.srv import Spawn
+from std_srvs.srv import Empty
+from turtlesim.srv import TeleportAbsolute
+
+from aruco_interfaces.msg import ArucoMarkerPose
+from turtlesim.msg import Pose
+from geometry_msgs.msg import Twist
+
+class TurtleFollowerPy(Node):
+
+    def __init__(self):
+        super().__init__('turtle_follower_node')
+
+        
+        #Create client for reset service
+        self.cli_turtlesim_reset = self.create_client(srv_type = Empty, srv_name = "/reset")
+        while not self.cli_turtlesim_reset.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('/reset service not available, waiting again...')
+        self.req_turtlesim_reset = Empty.Request()
+
+        #Create client for spawn service
+        self.cli_turtlesim_spawn = self.create_client(srv_type = Spawn, srv_name = "/spawn")
+        while not self.cli_turtlesim_spawn.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('/spawn service not available, waiting again...')
+        self.req_turtlesim_spawn = Spawn.Request()
+
+        #Create client for clear service
+        self.cli_turtlesim_clear = self.create_client(srv_type = Empty, srv_name = "/clear")
+        while not self.cli_turtlesim_clear.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('/clear service not available, waiting again...')
+        self.req_turtlesim_clear = Empty.Request()
+
+        #Create subscriber to get the current positions
+        self.sub_marker_pose = self.create_subscription(ArucoMarkerPose, "/aruco/aruco_poses", self.cbk_aruco_poses, 10)
+        self.sub_turtle1_pose = self.create_subscription(Pose, "/turtle1/pose", self.cbk_turtle1_pose ,10)
+        self.sub_turtle2_pose = self.create_subscription(Pose, "/turtle2/pose", self.cbk_turtle2_pose ,10)
+
+        #Create publisher to control turtle2 (follower)
+        self.pub_turtle2_motion = self.create_publisher(Twist, "/cmd_vel", 10)
+
+        #reset turtlesim and spawn second turtle
+        self.get_logger().info('resetting turtlesim ...')
+        self.reset_turtlesim()
+        self.target_pose = Pose()
+        self.current_pose = Pose()
+        self.target_posex = 1.0
+        self.target_posey = 1.0
+        self.target_pose.theta = math.pi
+        self.current_pose.x = 1.0
+        self.current_pose.y = 1.0
+        self.current_pose.theta = math.pi
+        self.spawn_turtle("turtle2", self.target_pose.x, self.target_pose.y, self.target_pose.theta) 
+        self.spawn_turtle("turtle2", self.current_pose.x, self.current_pose.y, self.current_pose.theta)
+
+        #Start timer for the follower controler
+        self.timer = self.create_timer(0.1, self.turtle2_follower_controler)
+
+    def spawn_turtle(self, name, x, y, theta):
+        self.req_turtlesim_spawn.x = x
+        self.req_turtlesim_spawn.y = y
+        self.req_turtlesim_spawn.theta = theta
+        self.req_turtlesim_spawn.name = name
+        future = self.cli_turtlesim_spawn.call_async(self.req_turtlesim_spawn)
+
+    def reset_turtlesim(self):
+        future = self.cli_turtlesim_reset.call_async(self.req_turtlesim_reset)
+
+    def cbk_aruco_poses(self, msg_pose):
+
+        #Create client for clear service
+        self.cli_turlesim_teleport = self.create_client(srv_type = TeleportAbsolute, srv_name = "turtle2/teleport_absolute")
+
+        #get the current coordinates and transform it to Turtlesim coordination system
+        self.req_turtlesim_teleport = TeleportAbsolute.Request()
+        self.req_turtlesim_teleport.x = (msg_pose.poses[0].position.x * 5) + 5.5
+        self.req_turtlesim_teleport.y = ((msg_pose.poses[0].position.y * -5) + 5.5)
+        self.req_turtlesim_teleport.theta = (math.asin(msg_pose.poses[0].orientation.z)*(-2) + math.pi/2)    # calculation of angle theta by asin, because of cosinus axis symmetry   
+        future = self.cli_turlesim_teleport.call_async(self.req_turtlesim_teleport)
+
+    def cbk_turtle1_pose(self, msg_pose):
+        self.target_pose.x = msg_pose.x
+        self.target_pose.y = msg_pose.y
+        self.target_pose.theta = msg_pose.theta
+
+
+    def cbk_turtle2_pose(self, msg_pose):
+        self.current_pose.x = msg_pose.x
+        self.current_pose.y = msg_pose.y
+        self.current_pose.theta = msg_pose.theta
+
+    def turtle2_follower_controler(self):
+        tolerance = 0.5
+        dist = math.sqrt(math.pow((self.target_pose.x - self.current_pose.x),2) + math.pow((self.target_pose.y - self.current_pose.y),2))
+        target_theta = math.atan2(self.target_pose.y - self.current_pose.y, self.target_pose.x - self.current_pose.x)
+        angle = target_theta - self.current_pose.theta
+
+        Ta = 0.1
+        kp_lin = 1.6/25
+        ki_lin = 1.1/25
+        kp_ang = 4*2/3
+        esum = 0
+        esum = esum + dist
+
+        #AntiWindup
+        esum_max = 2.0
+        if (esum > esum_max): esum = esum_max
+
+        #faster direction of rotation
+        if (angle > math.pi): angle = angle - (2*math.pi)
+        if (angle < -math.pi): angle = angle + (2*math.pi)
+
+        msg = Twist()
+        msg.linear.x = kp_lin * dist + ki_lin * Ta * esum;#dist
+        msg.linear.y = 0.0
+        msg.linear.z = 0.0
+        msg.angular.x = 0.0
+        msg.angular.y = 0.0
+        msg.angular.z = kp_ang*angle
+
+        if (abs(self.target_pose.x - self.current_pose.x) < tolerance and abs(self.target_pose.y - self.current_pose.y) < tolerance):
+            msg.linear.x = 0.0
+            msg.angular.z = 2 * (self.target_pose.theta - self.current_pose.theta)
+
+        if (abs(self.target_pose.x - self.current_pose.x) < tolerance and abs(self.target_pose.y - self.current_pose.y) < tolerance and abs(self.target_pose.theta - self.current_pose.theta)):
+            msg.linear.x = 0.0
+            msg.angular.z = 0.0
+            esum = 0.0  
+
+        self.pub_turtle2_motion.publish(msg)
+        self.get_logger().info("publishing")
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    turtle_follower = TurtleFollowerPy()
+
+    rclpy.spin(turtle_follower)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    turtle_follower.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1 @@
+0
@@ -0,0 +1,2 @@
+# generated from colcon_core/shell/template/command_prefix.sh.em
+. "/home/ros2/dev2_ws/install/aruco_interfaces/share/aruco_interfaces/package.sh"
@@ -0,0 +1,70 @@
+AMENT_PREFIX_PATH=/home/ros2/dev2_ws/install/v4l2_camera:/home/ros2/dev2_ws/install/turtle_follower_py:/home/ros2/dev2_ws/install/camera_calibration:/home/ros2/dev2_ws/install/aruco_interfaces:/home/ros2/dev2_ws/install/aruco_detector:/opt/ros/foxy
+CMAKE_PREFIX_PATH=/home/ros2/dev2_ws/install/aruco_interfaces:/home/ros2/dev2_ws/install/v4l2_camera
+COLCON=1
+COLCON_PREFIX_PATH=/home/ros2/dev2_ws/install
+COLORTERM=truecolor
+DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/1000/bus
+DESKTOP_SESSION=ubuntu
+DISPLAY=:0
+GDMSESSION=ubuntu
+GJS_DEBUG_OUTPUT=stderr
+GJS_DEBUG_TOPICS=JS ERROR;JS LOG
+GNOME_DESKTOP_SESSION_ID=this-is-deprecated
+GNOME_SHELL_SESSION_MODE=ubuntu
+GNOME_TERMINAL_SCREEN=/org/gnome/Terminal/screen/ed9a88cb_996e_4783_90fb_206aa627c957
+GNOME_TERMINAL_SERVICE=:1.166
+GPG_AGENT_INFO=/run/user/1000/gnupg/S.gpg-agent:0:1
+GTK_MODULES=gail:atk-bridge
+HOME=/home/ros2
+IM_CONFIG_PHASE=1
+INVOCATION_ID=331653316740409188736b6261a239d2
+JOURNAL_STREAM=8:183562
+LANG=en_US.UTF-8
+LC_ADDRESS=de_DE.UTF-8
+LC_ALL=en_US.UTF-8
+LC_IDENTIFICATION=de_DE.UTF-8
+LC_MEASUREMENT=de_DE.UTF-8
+LC_MONETARY=de_DE.UTF-8
+LC_NAME=de_DE.UTF-8
+LC_NUMERIC=de_DE.UTF-8
+LC_PAPER=de_DE.UTF-8
+LC_TELEPHONE=de_DE.UTF-8
+LC_TIME=de_DE.UTF-8
+LD_LIBRARY_PATH=/home/ros2/dev2_ws/install/aruco_interfaces/lib:/home/ros2/dev2_ws/install/v4l2_camera/lib:/opt/ros/foxy/opt/yaml_cpp_vendor/lib:/opt/ros/foxy/opt/rviz_ogre_vendor/lib:/opt/ros/foxy/lib/x86_64-linux-gnu:/opt/ros/foxy/lib
+LESSCLOSE=/usr/bin/lesspipe %s %s
+LESSOPEN=| /usr/bin/lesspipe %s
+LOGNAME=ros2
+LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
+MANAGERPID=7832
+OLDPWD=/home/ros2/dev2_ws/launch
+PAPERSIZE=a4
+PATH=/opt/ros/foxy/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
+PWD=/home/ros2/dev2_ws/build/turtle_follower_py
+PYTHONPATH=/home/ros2/dev2_ws/install/aruco_interfaces/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages:/opt/ros/foxy/lib/python3.8/site-packages
+QT_ACCESSIBILITY=1
+QT_IM_MODULE=ibus
+ROS_DISTRO=foxy
+ROS_LOCALHOST_ONLY=0
+ROS_PYTHON_VERSION=3
+ROS_VERSION=2
+SESSION_MANAGER=local/ubuntu:@/tmp/.ICE-unix/8042,unix/ubuntu:/tmp/.ICE-unix/8042
+SHELL=/bin/bash
+SHLVL=1
+SSH_AGENT_PID=8007
+SSH_AUTH_SOCK=/run/user/1000/keyring/ssh
+TERM=xterm-256color
+USER=ros2
+USERNAME=ros2
+VTE_VERSION=6003
+WINDOWPATH=2
+XAUTHORITY=/run/user/1000/gdm/Xauthority
+XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg
+XDG_CURRENT_DESKTOP=ubuntu:GNOME
+XDG_DATA_DIRS=/usr/share/ubuntu:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop
+XDG_MENU_PREFIX=gnome-
+XDG_RUNTIME_DIR=/run/user/1000
+XDG_SESSION_CLASS=user
+XDG_SESSION_DESKTOP=ubuntu
+XDG_SESSION_TYPE=x11
+XMODIFIERS=@im=ibus
+_=/usr/bin/colcon
@@ -0,0 +1,14 @@
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py/__init__.py
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py/turtle_follower_py.py
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py/__pycache__/__init__.cpython-38.pyc
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py/__pycache__/turtle_follower_py.cpython-38.pyc
+/home/ros2/dev2_ws/install/turtle_follower_py/share/ament_index/resource_index/packages/turtle_follower_py
+/home/ros2/dev2_ws/install/turtle_follower_py/share/turtle_follower_py/package.xml
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py-0.0.0-py3.8.egg-info/requires.txt
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py-0.0.0-py3.8.egg-info/top_level.txt
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py-0.0.0-py3.8.egg-info/dependency_links.txt
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py-0.0.0-py3.8.egg-info/PKG-INFO
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py-0.0.0-py3.8.egg-info/entry_points.txt
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py-0.0.0-py3.8.egg-info/zip-safe
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages/turtle_follower_py-0.0.0-py3.8.egg-info/SOURCES.txt
+/home/ros2/dev2_ws/install/turtle_follower_py/lib/turtle_follower_py/turtle_follower_node
@@ -0,0 +1,10 @@
+Metadata-Version: 1.2
+Name: turtle-follower-py
+Version: 0.0.0
+Summary: TODO: Package description
+Home-page: UNKNOWN
+Maintainer: ros2
+Maintainer-email: ros2@todo.todo
+License: TODO: License declaration
+Description: UNKNOWN
+Platform: UNKNOWN
@@ -0,0 +1,16 @@
+package.xml
+setup.cfg
+setup.py
+../../build/turtle_follower_py/turtle_follower_py.egg-info/PKG-INFO
+../../build/turtle_follower_py/turtle_follower_py.egg-info/SOURCES.txt
+../../build/turtle_follower_py/turtle_follower_py.egg-info/dependency_links.txt
+../../build/turtle_follower_py/turtle_follower_py.egg-info/entry_points.txt
+../../build/turtle_follower_py/turtle_follower_py.egg-info/requires.txt
+../../build/turtle_follower_py/turtle_follower_py.egg-info/top_level.txt
+../../build/turtle_follower_py/turtle_follower_py.egg-info/zip-safe
+resource/turtle_follower_py
+test/test_copyright.py
+test/test_flake8.py
+test/test_pep257.py
+turtle_follower_py/__init__.py
+turtle_follower_py/turtle_follower_py.py
@@ -0,0 +1 @@
+
@@ -0,0 +1,3 @@
+[console_scripts]
+turtle_follower_node = turtle_follower_py.turtle_follower_py:main
+
@@ -0,0 +1 @@
+setuptools
@@ -0,0 +1 @@
+turtle_follower_py
@@ -0,0 +1 @@
+