Initial commit, add all current packages

src/core_pkg/core_pkg/core_control_node.py

@@ -0,0 +1,198 @@
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Empty
+
+import signal
+import time
+import atexit
+
+import serial
+import sys
+import threading
+import glob
+
+from std_msgs.msg import String
+from ros2_interfaces_pkg.msg import CoreFeedback
+from ros2_interfaces_pkg.msg import CoreControl
+
+serial_pub = None
+thread = None
+
+class SerialRelay(Node):
+    def __init__(self):
+        # Initalize node with name
+        super().__init__("serial_publisher")#previously 'serial_publisher'
+
+        # Create publishers for feedback and telemetry
+        self.debug_pub = self.create_publisher(String, '/core/debug', 10) 
+        self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
+
+        # Create a subscriber to listen to any commands sent for the MCU
+        self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10)
+        
+
+        # Create a service server for pinging the rover
+        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
+
+        # Loop through all serial devices on the computer to check for the MCU
+        self.port = None
+        ports = SerialRelay.list_serial_ports()
+        for i in range(2):
+            for port in ports:
+                try:
+                    # connect and send a ping command
+                    ser = serial.Serial(port, 115200, timeout=1)
+                    print(f"Checking port {port}...")
+                    ser.write(b"ping\n")
+                    response = ser.read_until("\n")
+
+                    # if pong is in response, then we are talking with the MCU
+                    if b"pong" in response:
+                        self.port = port
+                        print(f"Found MCU at {self.port}!")
+                        break
+                except:
+                    pass
+            if self.port is not None:
+                break
+        
+        if self.port is None:
+            print("Unable to find MCU...")
+            time.sleep(1)
+            sys.exit(1)
+        
+        self.ser = serial.Serial(self.port, 115200)
+        atexit.register(self.cleanup)
+
+
+    def run(self):
+        # This thread makes all the update processes run in the background
+        global thread
+        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
+        thread.start()
+        
+        try:
+            while rclpy.ok():
+                self.read_MCU() # Check the MCU for updates
+        except KeyboardInterrupt:
+            sys.exit(0)
+
+    def read_MCU(self):
+        try:
+            output = str(self.ser.readline(), "utf8")
+            
+            if output:
+                # All output over debug temporarily
+                print(f"[MCU] {output}", end="")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+                return
+                # Temporary
+
+                # packet = output.strip().split(',')
+                
+                # if len(packet) >= 2 and packet[0] == "core" and packet[1] == "telemetry":
+                #     feedback = CoreFeedback()
+                #     feedback.gpslat = float(packet[2])
+                #     feedback.gpslon = float(packet[3])
+                #     feedback.gpssat = float(packet[4])
+                #     feedback.bnogyr.x = float(packet[5])
+                #     feedback.bnogyr.y = float(packet[6])
+                #     feedback.bnogyr.z = float(packet[7])
+                #     feedback.bnoacc.x = float(packet[8])
+                #     feedback.bnoacc.y = float(packet[9])
+                #     feedback.bnoacc.z = float(packet[10])
+                #     feedback.orient = float(packet[11])
+                #     feedback.bmptemp = float(packet[12])
+                #     feedback.bmppres = float(packet[13])
+                #     feedback.bmpalt = float(packet[14])
+
+                #     self.telemetry_publisher.publish(feedback)
+                # else:
+                #     # print(f"[MCU] {output}", end="")
+                #     # msg = String()
+                #     # msg.data = output
+                #     # self.debug_pub.publish(msg)
+                #     return
+        except serial.SerialException as e:
+            print(f"SerialException: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            self.exit(1)
+        except TypeError as e:
+            print(f"TypeError: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            self.exit(1)
+        except Exception as e:
+            print(f"Exception: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            self.exit(1)
+                
+    def scale_duty(self, value, max_speed):
+        leftMin = -1
+        leftMax = 1
+        rightMin = -max_speed/100.0
+        rightMax = max_speed/100.0
+
+
+        # Figure out how 'wide' each range is
+        leftSpan = leftMax - leftMin
+        rightSpan = rightMax - rightMin
+
+        # Convert the left range into a 0-1 range (float)
+        valueScaled = float(value - leftMin) / float(leftSpan)
+
+        # Convert the 0-1 range into a value in the right range.
+        return str(rightMin + (valueScaled * rightSpan))
+
+    def send_controls(self, msg):
+        #can_relay_tovic,core,19, left_stick, right_stick 
+        left_stick_neg = msg.left_stick * -1
+        command = "can_relay_tovic,core,19," + self.scale_duty(left_stick_neg, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
+        #print(f"[Sys] {command}", end="")
+        
+        self.ser.write(bytes(command, "utf8"))# Send command to MCU
+        self.get_logger().debug(f"wrote: {command}")
+        #print(f"[Sys] Relaying: {command}")
+
+
+    def ping_callback(self, request, response):
+        return response
+
+
+    @staticmethod
+    def list_serial_ports():
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+    
+    def cleanup(self):
+        print("Cleaning up before terminating...")
+        if self.ser.is_open:
+            self.ser.close()
+
+def myexcepthook(type, value, tb):
+    print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
+      
+
+
+def main(args=None):
+   rclpy.init(args=args)
+   sys.excepthook = myexcepthook
+
+   global serial_pub
+
+   serial_pub = SerialRelay()
+   serial_pub.run()
+
+if __name__ == '__main__':
+    signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+    main()
+

src/core_pkg/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>core_pkg</name>
+  <version>1.0.0</version>
+  <description>Core rover control package to handle command interpretation and embedded interfacing.</description>
+  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <license>All Rights Reserved</license>
+
+  <depend>rclpy</depend>
+  <depend>ros2_interfaces_pkg</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>

src/core_pkg/setup.cfg

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

src/core_pkg/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'core_pkg'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='tristan',
+    maintainer_email='tristanmcginnis26@gmail.com',
+    description='Core rover control package to handle command interpretation and embedded interfacing.',
+    license='All Rights Reserved',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            "core_control = core_pkg.core_control_node:main"
+        ],
+    },
+)

src/core_pkg/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'

src/core_pkg/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)

src/core_pkg/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'