adipu_ws

adipu_ws/src/adipu_msg/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.8)
+project(adipu_msg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Flip.msg"
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

adipu_ws/src/adipu_msg/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>adipu_msg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="adipu@utexas.edu">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

adipu_ws/src/adipu_turtlesim_controller/adipu_turtlesim_controller/mouse_follower.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from turtlesim.msg import Pose
+import tkinter as tk
+from std_srvs.srv import Empty
+from math import sqrt, pow, atan2, sin, cos
+
+CA = 10
+CL = 0.01
+
+def distFromGoal(x, y, centerx, centery):
+  return sqrt(pow(x - centerx, 2) + pow(y - centery, 2))
+
+def angleFromGoal(x, y, centerx, centery):
+  return atan2(y - centery, x - centerx)
+
+class Turtle1Controller(Node):
+  def __init__(self):
+    super().__init__("mouse_follow")
+    self.create_timer(0.1, self.publish_cmdVel)
+    self.root = tk.Tk()
+    self.cmdVelMsg=tk.StringVar()
+    self.pause_resume_info = tk.StringVar()
+    self.prStat = "PAUSE"
+    self.pause_resume_info.set(f"Click anywhere to {self.prStat} turtle1 control, right click to clear drawing")
+    self.paused = False
+    self.lin = 0.0
+    self.ang = 0.0
+    self.cmdPub = self.create_publisher(Twist, '/turtle1/cmd_vel', 1) # always most up-to-date, queue size 1
+    self.poseSub = self.create_subscription(Pose, '/turtle1/pose', self.update_theta, 1)
+    self.theta = 0
+    self.root.geometry("500x500")
+    self.root.title("Aditya's Turtle1 Controller")
+    self.canvas = tk.Canvas(self.root, width=500, height=500)
+    self.canvas.pack(fill=tk.BOTH, expand=True)
+    self.position_label = tk.Label(self.canvas, textvariable=self.cmdVelMsg)
+    self.position_label.pack(pady=20)
+    self.pause_resume_label = tk.Label(self.canvas, textvariable=self.pause_resume_info)
+    self.pause_resume_label.place(relx=0.5, rely=0.5, anchor=tk.CENTER)
+    self.root.bind('<Button-1>', self.pauseResume)
+    self.clear_client = self.create_client(Empty, '/clear')
+    self.root.bind('<Button-3>', self.clear_background)
+
+  def update_theta(self, data):
+    self.theta = data.theta
+
+  def publish_cmdVel(self):
+    if self.paused:
+      self.root.update_idletasks()
+      self.root.update()
+      return
+    
+    self.calcGoal()
+    msg = Twist()
+    msg.linear.x = self.lin
+    msg.angular.z = self.ang
+    self.cmdPub.publish(msg)
+
+    self.cmdVelMsg.set(f"lin.x: {self.lin:.2f}, ang.z: {self.ang:.2f}") #for tkinter display
+
+    self.root.update_idletasks()
+    self.root.update()
+
+  def calcGoal(self):
+    x = self.root.winfo_pointerx()
+    y = self.root.winfo_screenheight() - self.root.winfo_pointery()
+    center_x = self.root.winfo_screenwidth() // 2
+    center_y = self.root.winfo_screenheight() // 2
+    dist = distFromGoal(x, y, center_x, center_y)
+    angle = angleFromGoal(x, y, center_x, center_y)
+    self.ang = CA*atan2(sin(angle - self.theta), cos(angle - self.theta)) 
+    self.lin = CL * dist 
+    
+  def pauseResume(self, event):
+    self.paused = not self.paused
+    self.prStat = "RESUME" if self.paused else "PAUSE"
+    self.pause_resume_info.set(f"Click anywhere to {self.prStat} turtle1 control, right click to clear drawing")
+    msg = Twist()
+    msg.linear.x = 0.0
+    msg.angular.z = 0.0
+    self.cmdPub.publish(msg)
+    self.get_logger().info("Paused" if self.paused else "Resumed")
+
+  def clear_background(self, event):
+    if not self.clear_client.wait_for_service(timeout_sec=1.0):
+      self.get_logger().warn('/clear doesn\'t exist yet :(')
+      return
+    
+    req = Empty.Request()
+    asyncreq = self.clear_client.call_async(req)
+
+def main(args=None):
+  rclpy.init(args=args)
+
+  node = Turtle1Controller()
+  rclpy.spin(node)
+  rclpy.shutdown()
+
+if __name__=='__main__':
+  main()

adipu_ws/src/adipu_turtlesim_controller/adipu_turtlesim_controller/turtle1_chaser.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from turtlesim.msg import Pose
+from adipu_msg.msg import Flip
+from math import pow, atan2, sqrt, pi, sin, cos
+
+CL = 1
+CA = 5
+
+class Turtle2Controller(Node):
+  def __init__(self):
+    super().__init__("turtle_chase")
+    #always have latest data, set queue size to 1
+    self.turt1poseSub = self.create_subscription(Pose, '/turtle1/pose', self.update_goal, 1)
+    self.turt2poseSub = self.create_subscription(Pose, '/turtle2/pose', self.update_start, 1) 
+    self.turt2velPub = self.create_publisher(Twist, '/turtle2/cmd_vel', 1) 
+    self.turtFlipSub = self.create_subscription(Flip, '/turtle2/flipper', self.flip, 1)
+    self.runAway = False
+    self.create_timer(0.05, self.approach_goal)
+
+    self.pose = Pose()
+    self.goal_pose = Pose()
+    self.tolerance = 0.1
+    self.away_thresh = 7
+
+  def update_start(self, data):
+    self.pose = data
+    self.pose.x = round(self.pose.x, 4)
+    self.pose.y = round(self.pose.y, 4)
+
+  def update_goal(self, data):
+    self.goal_pose = data
+    self.goal_pose.x = round(self.goal_pose.x, 4)
+    self.goal_pose.y = round(self.goal_pose.y, 4)
+
+  def flip(self, data):
+    self.runAway = not self.runAway
+
+  def distFromGoal(self):
+    return sqrt(pow(self.goal_pose.x - self.pose.x, 2) 
+            + pow(self.goal_pose.y - self.pose.y, 
+              2)) if not self.runAway else self.away_thresh - sqrt(pow(self.goal_pose.x - self.pose.x, 2) 
+                                             + pow(self.goal_pose.y - self.pose.y, 2))
+  
+  def angleFromGoal(self):
+    return atan2(self.goal_pose.y - self.pose.y, 
+                 self.goal_pose.x - self.pose.x) if not self.runAway else pi + atan2(self.goal_pose.y - 
+                                                        self.pose.y, self.goal_pose.x - self.pose.x)
+  
+  def approach_goal(self):
+    turt2_cmd = Twist()
+    dist = self.distFromGoal()
+    angFromGoal = self.angleFromGoal()
+    # normalize angle difference, to account for -180 to 180 jump
+    ang = atan2(sin(angFromGoal - self.pose.theta), cos(angFromGoal - self.pose.theta))
+    directionalCoeff = 1 - abs(ang)*2/pi # filter movement so it only works constructively to our goal.
+    if dist >= self.tolerance:
+      turt2_cmd.linear.x = CL * dist * directionalCoeff
+      turt2_cmd.angular.z = CA*ang # proportional control, as outlined in https://wiki.ros.org/turtlesim/Tutorials/Go%20to%20Goal
+
+    else:
+      turt2_cmd.linear.x = 0.0
+      turt2_cmd.angular.z = 0.0
+
+    self.turt2velPub.publish(turt2_cmd)
+    self.get_logger().info(f"command: lin.x={turt2_cmd.linear.x:.2f}, ang.z={turt2_cmd.angular.z:.2f}")
+
+def main(args=None):
+  rclpy.init(args=args)
+  node = Turtle2Controller()
+  rclpy.spin(node)
+  rclpy.shutdown()
+
+if __name__=='__main__':
+  main()

adipu_ws/src/adipu_turtlesim_controller/adipu_turtlesim_controller/turtle2_flipper.py

@@ -0,0 +1,20 @@
+import rclpy
+from rclpy.node import Node
+from adipu_msg.msg import Flip
+
+class Turtle2Flipper(Node):
+  def __init__(self):
+    super().__init__("turtle_flip")
+    self.flipPub = self.create_publisher(Flip, '/turtle2/flipper', 1)
+    self.create_timer(10, self.flip)
+  
+  def flip(self):
+    msg = Flip()
+    self.flipPub.publish(msg)
+
+
+def main(args=None):
+  rclpy.init(args=args)
+  node = Turtle2Flipper()
+  rclpy.spin(node)
+  rclpy.shutdown()

adipu_ws/src/adipu_turtlesim_controller/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>adipu_turtlesim_controller</name>
+  <version>0.0.0</version>
+  <description>Package that includes mouse follower node for turtle 1, as well as chaser for turtle 2 and flipMode node for turtle 2 chasing/running away</description>
+  <maintainer email="adipu@utexas.edu">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>adipu_msg</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

adipu_ws/src/adipu_turtlesim_controller/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/adipu_turtlesim_controller
+[install]
+install_scripts=$base/lib/adipu_turtlesim_controller

adipu_ws/src/adipu_turtlesim_controller/setup.py

@@ -0,0 +1,28 @@
+from setuptools import find_packages, setup
+
+package_name = 'adipu_turtlesim_controller'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='root',
+    maintainer_email='adipu@utexas.edu',
+    description='Nodes for NOVA Onboarding',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+          "mouse_follow_node = adipu_turtlesim_controller.mouse_follower:main",
+          "chaser_node = adipu_turtlesim_controller.turtle1_chaser:main",
+          "chaser_flipper_node = adipu_turtlesim_controller.turtle2_flipper:main"
+        ]
+    }
+)

adipu_ws/src/adipu_turtlesim_controller/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

adipu_ws/src/adipu_turtlesim_controller/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

adipu_ws/src/adipu_turtlesim_controller/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'