Merge pull request #98 from UOA-FSAE/nightly

Merge nightly into stanley controller

src/action/acceleration/acceleration/controller.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+# Python imports
+import rclpy
+from rclpy.node import Node
+from moa_msgs.msg import ConeMap
+from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped
+from std_msgs.msg import Header
+
+
+class Acceleration_algorithm(Node):
+    def __init__(self):
+        super().__init__("Acceleration")
+        self.get_logger().info("Acceleration Started")
+
+        # subscribe to Cone detection result
+        self.create_subscription(ConeMap, "cone_detection", self.main_hearback, 5)
+
+        self.drive_pub = self.create_publisher(AckermannDrive, "/drive", 5)
+        self.drive_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5)
+        self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 5)
+
+
+    def main_hearback(self, msg: ConeMap):
+        if len(msg.cones) == 0:
+            #No cones, make throttle to 0
+            self.current_speed = 0.0
+            self.publish_ackermann()
+        else:
+            #There is cones, adjust throttle to target speed
+            self.current_speed = 20/3.6 #In m/s
+            self.publish_ackermann()
+
+
+
+    def publish_ackermann(self):
+
+        args1 = {"steering_angle": 0.0,
+                "steering_angle_velocity": 0.0,
+                "speed": float(self.current_speed),
+                "acceleration": 0.0,
+                "jerk": 0.0}
+        msg1 = AckermannDrive(**args1)
+
+        msg_cmd_vel = self.convert_to_stamped(msg1)
+        self.cmd_vel_pub.publish(msg_cmd_vel)
+        self.drive_pub.publish(msg1)
+        self.drive_vis_pub.publish(msg1)
+
+    def convert_to_stamped(self, ackermann_msgs):
+        stamped_msg = AckermannDriveStamped()
+        stamped_msg.header = Header()
+        stamped_msg.header.stamp = self.get_clock().now().to_msg()  # Set the current timestamp
+        stamped_msg.header.frame_id = '0'  # Set the appropriate frame ID
+        stamped_msg.drive = ackermann_msgs
+        return stamped_msg
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    accelrator = Acceleration_algorithm()
+
+    rclpy.spin(accelrator)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    accelrator.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/action/acceleration/package.xml

@@ -0,0 +1,18 @@
+<?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>acceleration</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yiyang030903@gmail.com">Yiyang Chen</maintainer>
+  <license>TODO: License declaration</license>
+
+  <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/action/acceleration/setup.cfg

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

src/action/acceleration/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'acceleration'
+
+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='Yiyang Chen',
+    maintainer_email='yiyang030903@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        	'controller = acceleration.controller:main',
+        ],
+    },
+)

src/action/acceleration/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/action/acceleration/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/action/acceleration/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'

src/moa/moa_bringup/launch/sim_launch.py

@@ -4,11 +4,18 @@
 def generate_launch_description():
     return launch.LaunchDescription([
         # cone map
-        # launch_ros.actions.Node(
-        #     package='cone_mapping',
-        #     executable='dbscan',
-        #     name='dbscan',
-        # ),
+        launch_ros.actions.Node(
+            package='aruco_detection',
+            executable='aruco_detection',
+            name='aruco_detection'
+        ), 
+
+        # cone mapping
+        launch_ros.actions.Node(
+            package='cone_mapping',
+            executable='dbscan',
+            name='dbscan',
+        ),
 
         # path optimization
         launch_ros.actions.Node(
@@ -19,8 +26,8 @@ def generate_launch_description():
 
         # path viz
         launch_ros.actions.Node(
-            package='path_planning_visualization',
-            executable='visualize2',
+            package='path_planning',
+            executable='shortest_path_viz',
             name='path_viz',
         ),
 
@@ -30,4 +37,11 @@ def generate_launch_description():
             executable='visualizer',
             name='track_viz',
         ),
+
+        # launch_ros.actions.Node(
+        #     package='foxglove_bridge',
+        #     executable='foxglove_bridge',
+        #     name='foxglove_bridge',
+        #     parameters=[{'port':8765}]
+        # ),
     ])

src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py

@@ -24,7 +24,7 @@ def __init__(self):
         #         ('debug', False)
         #     ]
         # )
-        self._distance_to_front = 3.0
+        self._distance_to_front = 0.9
 
         qos_profile = QoSProfile(
             reliability=QoSReliabilityPolicy.BEST_EFFORT,
@@ -84,7 +84,7 @@ def callback(self, msg:ConeMap):
             # publish msgs
             args = {"steering_angle": float(steering_angle_deg),
                     "steering_angle_velocity": 0.0,
-                    "speed": 7.0,
+                    "speed": 3.0,
                     "acceleration": 0.0,
                     "jerk": 0.0}
 

src/planning/path_planning/path_planning/shortest_path/CoreModels.py

@@ -11,7 +11,7 @@ def __init__(self, node, entryVector, velocity=0.0, cost=np.inf, previousNode=Fa
         self._previousNode = previousNode
         self._nextState = nextState
         self._max = Imax
-        self._min = Imin
+        # self._min = Imin
 
 class Node():
     def __init__(self, bid, xy, innerdistance, outerdistance) -> None:

src/planning/path_planning/path_planning/shortest_path/PathHelpers.py

@@ -35,3 +35,13 @@ def noughtTo60(nought_to_60):
     Converts nought to 60 to linear acceleration
     """
     return 60/nought_to_60 * 0.44704
+
+def getTraverseTime(distance_between_nodes, current_state, next_node_state):
+    dist = distance_between_nodes
+    c_vel = current_state._velocity # current state velocity
+    n_vel = next_node_state._velocity   # next state velocity
+    n_cost = next_node_state._cost  # next state cost
+    dist_bound = max(next_node_state._node._innerDistance, next_node_state._node._outerDistance)
+
+    objective = ((2*dist)/(c_vel+n_vel)) + n_cost + np.sqrt(dist_bound)
+    return objective

src/planning/path_planning/path_planning/shortest_path/TrackHelpers.py

@@ -151,10 +151,13 @@ def Plot(nodes:bool, vector_list:np.array, label:str,col=None):
 def plotOptimal(vector_list:np.array, velocities:np.array,  label:str):
     all_x = [P[0] for P in vector_list]
     all_y = [P[1] for P in vector_list]
-    col = cm.jet((velocities-np.min(velocities))/(np.max(velocities)-np.min(velocities)))
-    ax = plt.gca()
+    # col = cm.jet((velocities-np.min(velocities))/(np.max(velocities)-np.min(velocities)))
+    cmap = plt.cm.get_cmap("autumn_r")
+    ax = plt.subplot()
     for i in range(len(all_x)-1):
-        ax.plot([all_x[i], all_x[i+1]], [all_y[i], all_y[i+1]], c=col[i], label=label)
-    im = ax.scatter(all_x, all_y, c=velocities, s=0, cmap=cm.jet)
+        ax.plot([all_x[i], all_x[i+1]], [all_y[i], all_y[i+1]], c=cmap(velocities[i]/np.max(velocities)), label=label)
+    im = ax.scatter(all_x, all_y, c=velocities, s=0, cmap=cmap)
+    # im = cm.ScalarMappable(cmap=cmap)
+    # im = im.set_array(velocities)
 
     return im