diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..674237d Binary files /dev/null and b/.DS_Store differ diff --git a/safety_controller/.DS_Store b/safety_controller/.DS_Store new file mode 100644 index 0000000..b6c099c Binary files /dev/null and b/safety_controller/.DS_Store differ diff --git a/safety_controller/README.md b/safety_controller/README.md new file mode 100644 index 0000000..527a548 --- /dev/null +++ b/safety_controller/README.md @@ -0,0 +1,4 @@ +Safety_controller subscribes to ouster_points and imu_packets. +First, projects car's position in the next 1 second. +Second, takes the linear regression between curr position and future position. +Third, calculates distance from lidar points to line. If within crash delta (.2 m), then publishes STOP command. diff --git a/safety_controller/launch/testing.launch.py b/safety_controller/launch/testing.launch.py new file mode 100644 index 0000000..db46404 --- /dev/null +++ b/safety_controller/launch/testing.launch.py @@ -0,0 +1,39 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + safety_params = os.path.join( + get_package_share_directory('safety_controller'), + 'params.yaml' + ) + return LaunchDescription([ + Node( + package='safety_controller', + executable='tester', + name='safety_tester', + parameters=[ + {'drive_topic': '/vesc/high_level/input/nav_1'}, + ] + ), + Node( + package='safety_controller', + executable='controller', + name='safety_controller', + parameters=[ + safety_params, + {'scan_topic': '/scan'}, + {'drive_topic_listen': '/vesc/low_level/ackermann_cmd'}, + {'drive_topic_publish':'/vesc/low_level/input/safety'}, + ] + ), + Node( + package='safety_controller', + executable='logger', + name='error_logger', + parameters=[ + + ] + ) + ]) diff --git a/safety_controller/package.xml b/safety_controller/package.xml new file mode 100644 index 0000000..4245c3f --- /dev/null +++ b/safety_controller/package.xml @@ -0,0 +1,33 @@ + + + + safety_controller + 0.0.0 + TODO: Package description + racecar + TODO: License declaration + + ackermann_msgs + geometry_msgs + rclpy + sensor_msgs + std_msgs + visualization_msgs + tf2_ros + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + ackermann_msgs + geometry_msgs + rclpy + sensor_msgs + std_msgs + visualization_msgs + tf2_ros + + diff --git a/safety_controller/resource/safety_controller b/safety_controller/resource/safety_controller new file mode 100644 index 0000000..e69de29 diff --git a/safety_controller/safety_controller/__init__.py b/safety_controller/safety_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/safety_controller/safety_controller/__pycache__/__init__.cpython-310.pyc b/safety_controller/safety_controller/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..bc206a5 Binary files /dev/null and b/safety_controller/safety_controller/__pycache__/__init__.cpython-310.pyc differ diff --git a/safety_controller/safety_controller/__pycache__/logger_node.cpython-310.pyc b/safety_controller/safety_controller/__pycache__/logger_node.cpython-310.pyc new file mode 100644 index 0000000..14bbf08 Binary files /dev/null and b/safety_controller/safety_controller/__pycache__/logger_node.cpython-310.pyc differ diff --git a/safety_controller/safety_controller/__pycache__/safety_controller.cpython-310.pyc b/safety_controller/safety_controller/__pycache__/safety_controller.cpython-310.pyc new file mode 100644 index 0000000..de1f0ab Binary files /dev/null and b/safety_controller/safety_controller/__pycache__/safety_controller.cpython-310.pyc differ diff --git a/safety_controller/safety_controller/__pycache__/safety_tester.cpython-310.pyc b/safety_controller/safety_controller/__pycache__/safety_tester.cpython-310.pyc new file mode 100644 index 0000000..893a1bf Binary files /dev/null and b/safety_controller/safety_controller/__pycache__/safety_tester.cpython-310.pyc differ diff --git a/safety_controller/safety_controller/logger_node.py b/safety_controller/safety_controller/logger_node.py new file mode 100644 index 0000000..acdb408 --- /dev/null +++ b/safety_controller/safety_controller/logger_node.py @@ -0,0 +1,89 @@ +import rclpy +import random +from rclpy.node import Node + +from std_msgs.msg import Float32 +from sensor_msgs.msg import LaserScan +import pandas as pd +import numpy as np +import matplotlib.pyplot as plt +import os + +class WallFollowerLogger(Node): +# TODO: error metric: where we want it to stop - where it actually stopped +# TODO: publish error metric WRT to different velocities + + def __init__(self): + super().__init__('') + self.declare_parameters(namespace = '', parameters = [ + ('subscriber_topic', 'crash_points'), + ]) + + self.subscription = self.create_subscription( + LaserScan, + self.get_parameter('subscriber_topic').value, + self.listener_callback, + 10) + + rclpy.get_shutdown_context().on_shutdown(self.on_shutdown_callback) + + self.obstacle_sizes = [] + self.avg_distances = [] + + def on_shutdown_callback(self): + self.get_logger().info("--- Node is shutting down. Saving a log of the run. ---") + obstacle_time_axis = [0 + i for i in range(len(self.obstacle_sizes))] + distance_time_axis = [0 + i for i in range(len(self.avg_distances))] + + fig1 = plt.figure() + plt.plot(obstacle_time_axis,self.obstacle_sizes) + plt.figure('Obstacle Sizes Over Time During the Run') + plt.ylabel('Count of lidar points cluster') + plt.xlable('Timestep') + + fig2 = plt.figure() + plt.plot(distance_time_axis,self.avg_distances) + plt.figure('Average Distance To Obstacles During the Run') + plt.ylabel('Distance (m)') + plt.xlable('Timestep') + + fig1.savefig('obstacle_count') + fig2.savefig('avg_dist') + fig1.close() + fig2.close() + + obstacles_df = pd.DataFrame(self.obstacle_sizes) + distances_df = pd.DataFrame(self.avg_distances) + obstacles_df.to_csv('obstacle_count',index=False) + distances_df.to_csv('avg_dist',index=False) + self.get_logger().info(f'Saved csv files and plots to {os.getcwd()}/') + + def listener_callback(self, msg): + points = msg.ranges + + n = len(points) + if n > 0: + self.get_logger().info(f'Currently detecting a crash of size: {n}') + + avg_distance = np.mean(np.array(n)) + + self.obstacle_sizes.append(n) + self.avg_distances.append(avg_distance) + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = WallFollowerLogger() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/safety_controller/safety_controller/params.yaml b/safety_controller/safety_controller/params.yaml new file mode 100644 index 0000000..b8853b5 --- /dev/null +++ b/safety_controller/safety_controller/params.yaml @@ -0,0 +1,4 @@ +safety_controller: + ros__parameters: + safety_radius: 0.5 + safety_controller_const: 0.1 diff --git a/safety_controller/safety_controller/safety_controller.py b/safety_controller/safety_controller/safety_controller.py new file mode 100644 index 0000000..1baea1f --- /dev/null +++ b/safety_controller/safety_controller/safety_controller.py @@ -0,0 +1,258 @@ +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from ackermann_msgs.msg import AckermannDriveStamped +from visualization_msgs.msg import Marker + +from safety_controller.visualization_tools import VisualizationTools + +class SafetyController(Node): + + def __init__(self): + super().__init__("safety_controller") + # Declare parameters to make them available for use + # DO NOT MODIFY THIS! + self.declare_parameter("scan_topic", "/scan") + self.declare_parameter("drive_topic_listen", "/vesc/low_level/ackermann_cmd") # publish to the highest priority to override other drive commands + self.declare_parameter('drive_topic_publish', "/vesc/low_level/input/safety") + self.declare_parameter("safety_radius", 0.25) + self.declare_parameter("safety_controller_const", 0.25) + self.declare_parameter("logger_topic", "/crash_points") + + # Fetch constants from the ROS parameter server + # DO NOT MODIFY THIS! This is necessary for the tests to be able to test varying parameters! + self.SCAN_TOPIC = self.get_parameter('scan_topic').get_parameter_value().string_value + self.DRIVE_TOPIC_LISTEN = self.get_parameter('drive_topic_listen').get_parameter_value().string_value + self.DRIVE_TOPIC_PUBLISH = self.get_parameter('drive_topic_publish').get_parameter_value().string_value + self.SAFETY_RADIUS = self.get_parameter('safety_radius').get_parameter_value().double_value + self.SAFETY_CONTROLLER_CONST = self.get_parameter('safety_controller_const').get_parameter_value().double_value + self.LOGGER_TOPIC = self.get_parameter('logger_topic').get_parameter_value().string_value + ### Subscribers ### + self.lidar_subscriber = self.create_subscription( + LaserScan, + self.SCAN_TOPIC, + self.lidar_callback, + 10 + ) + + self.drive_subscriber = self.create_subscription( + AckermannDriveStamped, + self.DRIVE_TOPIC_LISTEN, + self.drive_callback, + 10 + ) + + ### Publishers ### + self.stop_publisher = self.create_publisher( + AckermannDriveStamped, + self.DRIVE_TOPIC_PUBLISH, + 10 + ) + + self.line_publisher = self.create_publisher( + Marker, + "line", + 10 + ) + + # the static transform from the base link frame to the lidar frame + self.base_to_lidar = np.array([ + [1.0, 0.0, 0.0, 0.275], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0] + ]) + + self.lidar_msg = None + + # TODO: Write your callback functions here + + def drive_callback(self, drive_msg): + """ + Based on a drive command, interrupt the drive command with a stop command + if it would lead to a collision. Collision determined based on a point in + the angle and distance range being too close to the projected path of the car + + Args: + - drive_msg (AckermannDriveStamped): AckermanDriveStamped msg from other controllers + """ + lidar_msg = self.lidar_msg + if lidar_msg is None: return + self.get_logger().info(f'{len(lidar_msg.ranges)}') + # function to filter our laser data + lidar_subset_calc = self.get_lidar_subset_calculator( + lidar_msg.angle_min, + lidar_msg.angle_max, + lidar_msg.angle_increment, + lidar_msg.ranges + ) + + # calculate the vector for our projected location + velocity = drive_msg.drive.speed + line = self.line_projection(velocity) + + # visualize the projected path to location in RViz + self.visualize_line(line) + + # filter laser scan to desired range + cartesian_coords = lidar_subset_calc( + angle_range = [-np.pi/4, np.pi/4], + distance_range= [0,np.linalg.norm(line)] + ) + # PROBLEM: this is giving us absolutely nothing + + # if any delta within the car safety radius, terminate the drive command + deltas = self.calculate_deltas(cartesian_coords, line) + self.get_logger().info(f'pre-masked deltas: {len(deltas)}') + mask = abs(deltas) < self.SAFETY_RADIUS + filtered_cartesian = deltas[mask] + self.get_logger().info(f'Deltas: {filtered_cartesian}') + if len(filtered_cartesian) > 0: + self.publish_stop() + + def lidar_callback(self, lidar_msg): + self.lidar_msg = lidar_msg + + def publish_stop(self): + """ + Publishes a command for the car to stop. + """ + # https://docs.ros.org/en/jade/api/ackermann_msgs/html/msg/AckermannDriveStamped.html + new_msg = AckermannDriveStamped() + + # https://docs.ros.org/en/jade/api/ackermann_msgs/html/msg/AckermannDrive.html + drive_command = new_msg.drive + drive_command.speed = 0.0 + drive_command.acceleration = 0.0 + # jerk indicates a desired absolute rate of acceleration change in either direction (increasing or decreasing). + drive_command.jerk = 0.0 + + self.stop_publisher.publish(new_msg) + + def get_lidar_subset_calculator(self, lidar_angle_min, lidar_angle_max, lidar_angle_increment, lidar_ranges): + """ + Returns a function tuned to the lidar's base parameters (lidar_angle_min, lidar_angle_max, and lidar_angle_increment) + that returns points within an angle range. + + Args: + - lidar_angle_min (float): The minimum angle supported by the lidar + - lidar_angle_max (float): The maximum angle supported by the lidar + - lidar_angle_increment (float): The size of the increments in the range [lidar_angle_min, lidar_angle_max] + - lidar_ranges (array): The original lidar data, an indexed by angle with values corresponding to the distance of the point + from the lidar + """ + def subset_calculator(angle_range = [lidar_angle_min, lidar_angle_max], distance_range = [0, float("inf")]): + """ + Returns the cartesian coordinates of the lidar points within a given range of angles + and a given distance range. + + Args: + angle_range (array): The given range of angles + distance_range (array): The given range of distance + """ + angle_min, angle_max = angle_range + distance_min, distance_max = distance_range + + # Angle Subset + if angle_min > angle_max: + # Swap angles if given in wrong order + angle_min, angle_max = angle_max, angle_min + + # Clip angles to the minimum and maximum angles supported by the lidar + angle_min = max(angle_min, lidar_angle_min) + angle_max = min(angle_max, lidar_angle_max) + + # Compute the range indices associated with the minumum and maximum angle + range_low_index = int((angle_min - lidar_angle_min) / lidar_angle_increment) + range_high_index = int((angle_max - lidar_angle_min) / lidar_angle_increment) + + # Create array of indices associated with each value in our valid range + desired_indices = np.arange(range_low_index,range_high_index+1) + # Scale to increment based on given value, giving us each angle + corresponding_angles = lidar_angle_min + desired_indices * lidar_angle_increment + # Use the min and max index values to clip ranges to valid points + corresponding_ranges = np.array(lidar_ranges[range_low_index:range_high_index+1], dtype="float32") + + # Distance mask to filter out points outside given range + distance_mask = (distance_min <= corresponding_ranges) & (corresponding_ranges <= distance_max) + + # Apply distance mask to angles + valid_angles = corresponding_angles[distance_mask] + # Apply distance mask to ranges + valid_ranges = corresponding_ranges[distance_mask] + + # Convert valid points to Cartesian coordinates + x = valid_ranges * np.cos(valid_angles) + y = valid_ranges * np.sin(valid_angles) + + # Stack Cartestian coordinates together and transpose + cartesian_coords = np.vstack((x, y)).T # Shape: (num_points, 2) + + return cartesian_coords + + return subset_calculator + + def visualize_line(self, line_end_point): + """ + Returns None + + Visualizes the line starting at (0,0) to the given end point in + the base_link frame. + + Args: + - line_end_point (ndarray) : the (x,y) of the end point + """ + + end_x, end_y = line_end_point + VisualizationTools.plot_line( + x = [0.0, float(end_x)], + y = [0.0, float(end_y)], + publisher = self.line_publisher, + ) + + def line_projection(self, velocity): + """ + Returns the vector representing the projected location of base_link with respect to base_link. + + Args: + - velocity (float) : the current velocity of the drive command + + Output: + - line (ndarray): (1, 2) 2d vector (x, y) of the end point of the projected line. + """ + projected_distance = self.SAFETY_CONTROLLER_CONST * velocity + self.SAFETY_RADIUS + line = np.array([projected_distance, 0]) # x direction is forward + return line + + def calculate_deltas(self, coords, line): + """ + Takes in the lidar scan points in the desired range and calculates their + distance to the line of our projected path for base_link. + + Args: + - coords (ndarray): (num_points, 2) Cartesian coordinates of each scan point w.r.t. base link. + - line (ndarray) : (1, 2) Vector to the projected location of base_link. + """ + self.get_logger().info(f'calculate_deltas input: {len(coords)}') + + # Calculate the unit vector associated with the line + unit_vec = line/np.linalg.norm(line) + + # Calculate the cross product between each coordinate and the unit vector + # This gets distances to the projected path line + deltas = np.cross(coords, unit_vec) + + # Return distances to projected path line + return deltas + +def main(): + rclpy.init() + safety_controller = SafetyController() + rclpy.spin(safety_controller) + safety_controller.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/safety_controller/safety_controller/safety_tester.py b/safety_controller/safety_controller/safety_tester.py new file mode 100644 index 0000000..7d7a3bd --- /dev/null +++ b/safety_controller/safety_controller/safety_tester.py @@ -0,0 +1,60 @@ +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from ackermann_msgs.msg import AckermannDriveStamped + +class SafetyTester(Node): + + def __init__(self): + super().__init__("safety_tester") + # Declare parameters to make them available for use + # DO NOT MODIFY THIS! + self.declare_parameter("drive_topic", "/vesc/high_level/input/nav_1") + self.declare_parameter("drive_speed",1.0) + + # Fetch constants from the ROS parameter server + # DO NOT MODIFY THIS! This is necessary for the tests to be able to test varying parameters! + self.DRIVE_TOPIC = self.get_parameter('drive_topic').get_parameter_value().string_value + self.SPEED = self.get_parameter('drive_speed').get_parameter_value().double_value + + self.timer = self.create_timer(0.05, self.drive_timer_callback) + ### Publishers ### + self.drive_test_publisher = self.create_publisher( + AckermannDriveStamped, + self.DRIVE_TOPIC, + 10 + ) + + # # https://docs.ros.org/en/jade/api/ackermann_msgs/html/msg/AckermannDriveStamped.html + # new_msg = AckermannDriveStamped() + + # # https://docs.ros.org/en/jade/api/ackermann_msgs/html/msg/AckermannDrive.html + # drive_command = new_msg.drive + # drive_command.speed = 1.0 + # drive_command.acceleration = 0.0 + # # jerk indicates a desired absolute rate of acceleration change in either direction (increasing or decreasing). + # drive_command.jerk = 0.0 + + # self.drive_test_publisher.publish(new_msg) + + def drive_timer_callback(self): + """ Basic Drive Command """ + new_msg = AckermannDriveStamped() + drive_command = new_msg.drive + drive_command.speed = self.SPEED + drive_command.acceleration = 0.0 + drive_command.jerk = 0.0 + self.drive_test_publisher.publish(new_msg) + + +def main(): + rclpy.init() + safety_tester = SafetyTester() + rclpy.spin(safety_tester) + safety_tester.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/safety_controller/safety_controller/visualization_tools.py b/safety_controller/safety_controller/visualization_tools.py new file mode 100644 index 0000000..170f8af --- /dev/null +++ b/safety_controller/safety_controller/visualization_tools.py @@ -0,0 +1,43 @@ +from geometry_msgs.msg import Point +from visualization_msgs.msg import Marker + + +class VisualizationTools: + + @staticmethod + def plot_line(x, y, publisher, color=(1.0, 0.0, 0.0), frame="/base_link"): + """ + Publishes the points (x, y) to publisher + so they can be visualized in rviz as + connected line segments. + Args: + x, y: The x and y values. These arrays + must be of the same length. + publisher: the publisher to publish to. The + publisher must be of type Marker from the + visualization_msgs.msg class. + color: the RGB color of the plot. + frame: the transformation frame to plot in. + """ + # Construct a line + line_strip = Marker() + line_strip.type = Marker.LINE_STRIP + line_strip.header.frame_id = frame + + # Set the size and color + line_strip.scale.x = 0.1 + line_strip.scale.y = 0.1 + line_strip.color.a = 1.0 + line_strip.color.r = color[0] + line_strip.color.g = color[1] + line_strip.color.b = color[2] + + # Fill the line with the desired values + for xi, yi in zip(x, y): + p = Point() + p.x = xi + p.y = yi + line_strip.points.append(p) + + # Publish the line + publisher.publish(line_strip) diff --git a/safety_controller/setup.cfg b/safety_controller/setup.cfg new file mode 100644 index 0000000..c015f27 --- /dev/null +++ b/safety_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/safety_controller +[install] +install_scripts=$base/lib/safety_controller diff --git a/safety_controller/setup.py b/safety_controller/setup.py new file mode 100644 index 0000000..94a6c71 --- /dev/null +++ b/safety_controller/setup.py @@ -0,0 +1,43 @@ +import glob +import os + +from setuptools import find_packages, setup + +package_name = 'safety_controller' + +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']), + ( + 'share/' + package_name, + ['safety_controller/params.yaml'], + ), + ( + 'share/' + package_name + '/launch', + glob.glob(os.path.join('launch', '*.launch.py')), + ), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='racecar', + maintainer_email='racecar@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'controller = safety_controller.safety_controller:main', + 'tester = safety_controller.safety_tester:main', + 'logger = safety_controller.logger_node:main' + ], + }, +) diff --git a/safety_controller/test/test_copyright.py b/safety_controller/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/safety_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' diff --git a/safety_controller/test/test_flake8.py b/safety_controller/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/safety_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) diff --git a/safety_controller/test/test_pep257.py b/safety_controller/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/safety_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' diff --git a/tmux.yaml b/tmux.yaml new file mode 100644 index 0000000..e23328f --- /dev/null +++ b/tmux.yaml @@ -0,0 +1,14 @@ +session_name: rss_lab1c +windows: + - window_name: team2_tmux_template + layout: tiled + panes: + - shell_command: + - colcon build --symlink-install + - source install/setup.bash + - shell_command: + - source install/setup.bash + - shell_command: + - source install/setup.bash + - shell_command: + - source install/setup.bash diff --git a/wall_follower_real/.gitignore b/wall_follower_real/.gitignore new file mode 100644 index 0000000..8bb77a0 --- /dev/null +++ b/wall_follower_real/.gitignore @@ -0,0 +1,3 @@ +*.sw* +*.pyc +*.npz diff --git a/wall_follower_real/README.md b/wall_follower_real/README.md new file mode 100644 index 0000000..3875916 --- /dev/null +++ b/wall_follower_real/README.md @@ -0,0 +1,226 @@ +| **Deliverable** | **Due Date** | +|--------------------------------------------------------------------------------------------------------------|----------------------------------------| +| 6x `TEST_NAME_log.npz` files on [Gradescope](https://www.gradescope.com/courses/1227626/assignments/7471278) | Wednesday, February 25th at 1:00PM EST | +| In-person Lab Checkoffs | Wednesday, February 25th, during lab | + +# Lab 2: Wall Following in Simulation + +## Table of Contents +- [Introduction](#introduction) +- [Download this Repository](#download-this-repository) +- [Running the Simulator/Wall Follower](#running) +- [Lab Overview](#lab-overview) +- [Steps to Success](#steps-to-success) +- [Scoring](#scoring) +- [Submission](#submission) +- [Troubleshooting and Other Notes](#troubleshooting-and-other-notes) +- [Frequently Used Instructions](https://github.com/mit-rss/frequently_used_instructions) + + +
+ +## ⚠️ Before You Start Lab 2 (Required Setup) + +Before starting Lab 2, you **must** pull the latest Docker image. We have made some modifications since Lab 1 that will make things run smoother. + +From your **host machine** (not inside the Docker container), navigate to your `racecar_docker` folder and run: + + docker compose pull + +## Introduction + +[[Link to Lab Slides]](https://docs.google.com/presentation/d/1sp8WougmLo8mEJXdvfs6KyGxbIw56jGo/edit) + +In this lab, you will be implementing a wall follower on a simulated racecar. Your goal is to make an autonomous controller that drives the racecar forwards while maintaining a constant distance from a wall on either its left or right. We will set parameter to determine which side of the car we want to follow the wall, but the wall itself will be detected on the fly. It should be robust to uneven surfaces and small errors in the LIDAR data, and it should be able to recover from deviations from the desired state; being too far, too close, or not parallel to the wall. + +Thumbnail GIF + +This lab should be done individually. In the next lab, you will join your team to get your wall follower working on the real racecar. You may reuse code you write for this lab so make sure it is clean and presentable to your teammates! + +We have made a series of tests to evaluate the performance of your wall follower. In order to test your code properly you must start with the template starter code. The template is still very sparse and you have plenty of freedom to implement any algorithm you'd like so long as the inputs and outputs are the same. + +
+ +## Download this Repository + +All instructions detailed here should happen in your docker container. + +Clone this repository into your colcon workspace: + + cd ~/racecar_ws/src + git clone https://github.com/mit-rss/wall_follower_sim.git + +Then rebuild your workspace with `colcon build`: + + cd ~/racecar_ws + colcon build --symlink-install + source install/setup.bash + +We ask that you use the starter code contained in this repo and do not tweak its original structure. Make sure that your wall follower lives in the ROS node initialized in the Python script at: + + wall_follower/wall_follower.py + +However, feel free to add more Python files to keep your code organized. + + +
+ +## Running + +### 1. Running the Simulator + +All instructions detailed here should happen in your docker container. + +First, open [`rviz2`](https://docs.ros.org/en/humble/p/rviz2/) using the right-click menu in the NoVNC browser graphical interface (which can be accessed at http://localhost:6080/vnc.html?resize=remote after the docker container is started). + +Launch the [racecar simulator](https://github.com/mit-racecar/racecar_simulator) by running (from any directory): + + ros2 launch racecar_simulator simulate.launch.xml + +Note that you must open RViz ***before*** you launch the racecar simulator for the map to appear (since the map only is published once when the simulator is started). + +You should see a car in a map (walls are black, empty space is grey) and colorful points on that map representing lidar detections. + +![The racecar in the starting position](https://raw.githubusercontent.com/mit-racecar/racecar_simulator/master/media/racecar_simulator_rviz_1.png) + +You can change the position of the robot by clicking the "2D Pose Estimate" button on top of RViz and placing the arrow somewhere on the map. + +As you see in the map on your screen, we're currently simulating the environment of the Stata basement. While you can certainly use this environment to develop your wall follower, for our tests, we'll be using a more challenging environment: the Building 31 basement. If you'd like to see what that looks like, you can kill the simulator above, and run the following command: + + ros2 launch wall_follower launch_test_sim.launch.py + +You're welcome to use either environment to test your wall follower, but be sure to only have one simulator process running at a time. In Step 2, you will have to use the Building 31 environment to run our test cases. + +### 2. Running your Wall Follower + +While running your simulator, open up another terminal and run: + + ros2 launch wall_follower wall_follower.launch.xml + +***Note that you can modify and rerun your wall follower without needing to restart the simulator. Run the simulator once... and leave it running.*** + +### 3. Running the Tests + +There are 6 test cases, with varying start and end positions, velocities, wall-side, and desired distances. + +You will run your tests locally and submit the log.npz files that the tests generate. + +First, launch the test simulator: + + ros2 launch wall_follower launch_test_sim.launch.py + +Then, in a separate terminal, launch the tests (which will automatically launch your wall follower as well): + + ros2 launch wall_follower launch_test.launch.py + +***Right now, you should have at least 2 terminals open. Use the [tmux template](https://github.com/mit-rss/intro_to_linux/blob/master/tmux_template.yaml) from Lab 1 to see all of your terminals in the same window.*** + +You can view the tests running in RViz. Note that you can visualize the target end position by adding the "/end_position_marker" topic to RViz. + +For an example of how the tests should look when running, see [this video](https://youtu.be/r7ygU1zlTjU). + +If you're curious, the tester code is in `/wall_follower_sim/wall_follower/test_wall_follower.py` + +**Note that, while the simulator does not simulate collisions, the autograder will check that your car has not crashed.** + +
+ +## Lab Overview + +All instructions detailed here should happen in your docker container. All of the work that you will do in this lab will be by modifying the wall_follower.py file. + +### 1. Send Drive Commands + +To make the car drive, publish messages of type [`AckermannDriveStamped`](http://docs.ros.org/jade/api/ackermann_msgs/html/msg/AckermannDriveStamped.html) to the `/drive` topic. + +Import the `AckermannDriveStamped` type like this in your `wall_follower.py` file: + + from ackermann_msgs.msg import AckermannDriveStamped + +### 2. Read LIDAR Data + +The racecar has a 2D LIDAR sensor that measures distances from the racecar to its surroundings. All LIDAR data is published to the `/scan` topic by the simulator (so you should only need to subscribe to this topic). + +The data is of type [`LaserScan`](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html). You can import the type in python using: + + from sensor_msgs.msg import LaserScan + +The `ranges` entry in the `LaserScan` message is an array of the distances from the lidar sensor to the nearest obstacle. The measurements are taken at regular angular intervals of `angle_increment` radians, from the angle `angle_min` to `angle_max`. + +The rainbow points in this image below are the laser scan as visualized in RViz. The color corresponds to the intensity of the scan. In the simulator this is simply the distance, but on the actual lidar it gives you an indication of how reflective the object you are scanning is. Note that there is no data in the quadrant behind the car because the LIDAR sensor does not scan the full 360 degree range. + +![The racecar in a cubicle](https://raw.githubusercontent.com/mit-racecar/racecar_simulator/master/media/racecar_simulator_rviz_2.png) + +### 3. Use the Simulator Parameters + +There is a [params.yaml](https://github.com/mit-rss/wall_follower_sim/blob/master/wall_follower/params.yaml) file that defines a few parameters for your wall-follower. Most notably: + +* `desired_distance`: distance that the racecar should maintain from the wall +* `velocity`: speed the racecar should move in meters per second +* `side`: The side the wall is following represented as an integer. +1 represents the left wall and -1 represents the right wall. We chose this convention because typically we will assume the car is pointing in the positive _x_ direction. That means the left side of the car will point to the positive _y_ axis and the right side will point to the negative _y_ axis. + +Not only is this param file useful for efficiently testing different configurations, **it is NECESSARY for the autograder**! Therefore, your wall follower must fetch and use these parameters so that the autograder can test the various test cases. + +(Note: the `scan_topic` and `drive_topic` parameters are optional, though defining topic names in a param file is generally good practice). + +
+ +## Steps to Success +How you implement the wall follower is entirely up to you. However, the basic idea is that you will subscribe to lidar data, compute an estimate of where the wall is relative to the car, compute a control command from that estimated location, and then publish the control command to move the car. However, these are some key tips we compiled that will set you in the right direction: + +* __One step at a time__: Begin by setting up your wall follower node so that it subscribes to laser messages and publishes steering commands. Make sure you can move the racecar at a constant speed and turning angle before working on your controller. +* __Slice up the scan__: Only a subset of the laserscan points will be useful to you -- how you filter these points will ***significantly*** impact your wall follower's performance. Think carefully about which laserscan points matter -- should you threshold by angle? by distance? by x- and y- coordinate (relative to the `base_link` frame)? Seriously, draw it out (based on the test cases) and think it through. When filtering the laserscan points, Try to use [```numpy```](https://numpy.org/) operations rather than for loops in your code. [Multidimensional slicing](https://docs.scipy.org/doc/numpy-1.13.0/reference/arrays.indexing.html) and [broadcasting](https://docs.scipy.org/doc/numpy-1.13.0/user/basics.broadcasting.html) can make your code cleaner and much more efficient. +* __Find the wall__: In a perfect world, you might be able to detect the wall by fitting a line to 2 samples of the LIDAR data. However with noisy data and uneven surfaces this might not be enough. A [least squares regression](https://en.wikipedia.org/wiki/Simple_linear_regression) is an easy way to account for more noise. The [RANSAC](https://en.wikipedia.org/wiki/Random_sample_consensus) outlier-rejection algorithm can further “upgrade” an existing model (like least squares). _Note: Attempt RANSAC only if you've already built a functional wall follower. It is probably overkill._ +* __Use PD or PID control__: There are multiple ways to implement your control logic; for example, PID control can be used to stabilize your robot to a fixed distance from a wall, while Pure Pursuit with Ackermann dynamics can be used to follow a path defined by the wall. While both methods work, PD/PID control is more well-known for handling disturbances like curved walls and corners. Simple P (proportional) control is often not enough to create a responsive and stable system. Tuning the constants of this system can be done through empirical observations or more [systematically](https://www.crossco.com/resources/technical/how-to-tune-pid-loops/). +* __Use the visualization code__: We provided an example Python script in `wall_follower` that plots a line in Rviz. You can write something similar to this in order to make sure your code (e.g. wall detection) is working! + +## Scoring + +In each test case we compute distances from the racecar to the wall on the appropriate side at regular time intervals. +If your code is truly following a wall than it should minimize the average absolute difference between the distance to the wall and the desired distance to the wall. + +$$loss=\frac{1}{N}\sum_{i=0}^N\left|distance[i]-desired\\_distance\right|$$ + +To turn this value into a score between 0 and 1 we compute: + +$$score=\frac{1}{1+(\alpha\cdot{loss})^2}$$ + +Don't worry, it is impossible to get exactly 100%. +In some test cases we start the racecar closer or farther to the wall than the desired distance, which will automatically lower the max score. +The racecar also has to navigate around tight turns which it can't do perfectly with a limited turning radius. +Moreover there are many ways to measure the distance to a wall so our metric might not match yours exactly. +We have chosen $\alpha$, so your score will be in the high 90's for most of the tests. +Your score for the `short_left_far_angled` test will be lower than the others because the racecar starts far from the desired distance. Example TA grades below: + +![TA grades](https://c2.staticflickr.com/8/7882/33284090908_e04084e7d6_o.png) + +## Submission + +Running the tests (see [[3. Running the Tests]](https://github.com/mit-rss/wall_follower_sim#3-running-the-tests)) will generate 6 log files that will appear in your ROS2 workspace home: `racecar_docker/home/racecar_ws/TEST_NAME_log.npz` Submit all 6 test files to the [Gradescope assignment](https://www.gradescope.com/courses/1227626/assignments/7471278). + +(If you have not generated all the files because you have not passed all the tests, you can still get partial points from submitting whatever files you do have.) + +## Troubleshooting and Other Notes + +#### When in doubt: + +Try restarting both the simulator and your wall follower node. We've seen strange issues where the laser scan publisher seems to stop working... a simple restart of the simulator node fixes this. + +#### Simulator Parameters + +You may have noticed that the simulator has a few parameters defined in [params.yaml](https://github.com/mit-racecar/racecar_simulator/blob/master/params.yaml): + +- `max_speed`: 4 meters/second +- `max_steering_angle`: 0.34 radians + +You generally should not modify these; they will be reset by the autograder. + +
+ + +#### If you don't see the car appearing in the RViz simulation: +Firstly, confirm that the simulator (`ros2 launch racecar_simulator simulate.launch.xml`) is running successfully. + +If so, if you're using the [racecar docker image](https://github.com/mit-racecar/racecar_docker), Rviz will already be configured to visualize the simulator. But if not, in the left panel on the bottom click the "Add" button, and then in the "By display type" tab click "RobotModel". You should see a small blue car appear. Then click "Add" again and in the "By topic" tab click add the "/map" topic. Repeat once more to add the laser scan topic. Under the dropdown menu of your LaserScan there should be a field called "Size (m)". Change this to 0.1 so you can see the laser scan more clearly. The checkboxes turn on and off display types, which may be useful as you add topics to visualize. + +![Add button](https://i.imgur.com/85tY4tZ.png) diff --git a/wall_follower_real/launch/launch_test.launch.py b/wall_follower_real/launch/launch_test.launch.py new file mode 100644 index 0000000..55d51f6 --- /dev/null +++ b/wall_follower_real/launch/launch_test.launch.py @@ -0,0 +1,604 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch_ros.actions import Node +from launch.event_handlers import (OnExecutionComplete, OnProcessExit, + OnProcessIO, OnProcessStart, OnShutdown) +from launch.actions import (ExecuteProcess, + LogInfo, RegisterEventHandler, TimerAction) +from launch.substitutions import FindExecutable + +import os +from ament_index_python.packages import get_package_share_directory +import numpy as np + + +def generate_launch_description(): + ############################################################################ + ### Define test nodes and all their parameters + ############################################################################ + test1 = Node( + package="wall_follower_rover", + executable="test_wall_follower", + namespace='test1', + parameters=[ + {"scan_topic": "/scan", + "drive_topic": "/drive", + "pose_topic": "/pose", + "velocity": 1., + "desired_distance": 1., + "side": -1, + "start_x": -4., + "start_y": -5.4, + "start_z": 0.0, + "end_x": 5.0, + "end_y": -5.0, + "name": "short_right_close"}], + name='test_wall_follower', + remappings=[ + ('/test1/pose', '/pose'), + ('/test1/map', '/map'), + ('/test1/base_link', '/base_link'), + ('/test1/tf', '/tf'), + ('/test1/tf_static', '/tf_static'), + ] + ) + + test2 = Node( + package="wall_follower_rover", + executable="test_wall_follower", + namespace='test2', + parameters=[ + {"scan_topic": "/scan", + "drive_topic": "/drive", + "pose_topic": "/pose", + "velocity": 1., + "desired_distance": 1., + "side": 1, + "start_x": 5.0, + "start_y": -4.4, + "start_z": np.pi - 0.001, + "end_x": -4.0, + "end_y": -5.0, + "name": "short_left_far"}], + name='test_wall_follower', + remappings=[ + ('/test2/pose', '/pose'), + ('/test2/map', '/map'), + ('/test2/base_link', '/base_link'), + ('/test2/tf', '/tf'), + ('/test2/tf_static', '/tf_static'), + ] + ) + + test3 = Node( + package="wall_follower_rover", + executable="test_wall_follower", + namespace='test3', + parameters=[ + {"scan_topic": "/scan", + "drive_topic": "/drive", + "pose_topic": "/pose", + "velocity": 2., + "desired_distance": 1., + "side": -1, + "start_x": -4.0, + "start_y": -5.0, + "start_z": -np.pi/4, + "end_x": 5.0, + "end_y": -5.0, + "name": "short_right_angled"}], + name='test_wall_follower', + remappings=[ + ('/test3/pose', '/pose'), + ('/test3/map', '/map'), + ('/test3/base_link', '/base_link'), + ('/test3/tf', '/tf'), + ('/test3/tf_static', '/tf_static'), + ] + ) + + test4 = Node( + package="wall_follower_rover", + executable="test_wall_follower", + namespace='test4', + parameters=[ + {"scan_topic": "/scan", + "drive_topic": "/drive", + "pose_topic": "/pose", + "velocity": 2., + "desired_distance": 1., + "side": 1, + "start_x": 5.0, + "start_y": -4.0, + "start_z": 3*np.pi/4., + "end_x": -4.0, + "end_y": -5.0, + "name": "short_left_far_angled"}], + name='test_wall_follower', + remappings=[ + ('/test4/pose', '/pose'), + ('/test4/map', '/map'), + ('/test4/base_link', '/base_link'), + ('/test4/tf', '/tf'), + ('/test4/tf_static', '/tf_static'), + ] + ) + + test5 = Node( + package="wall_follower_rover", + executable="test_wall_follower", + namespace='test5', + parameters=[ + {"scan_topic": "/scan", + "drive_topic": "/drive", + "pose_topic": "/pose", + "velocity": 2., + "desired_distance": 1., + "side": -1, + "start_x": -4.0, + "start_y": -5.4, + "start_z": -np.pi/6., + "end_x": -3.5, + "end_y": 17.6, + "name": "long_right"}], + name='test_wall_follower', + remappings=[ + ('/test5/pose', '/pose'), + ('/test5/map', '/map'), + ('/test5/base_link', '/base_link'), + ('/test5/tf', '/tf'), + ('/test5/tf_static', '/tf_static'), + ] + ) + + test6 = Node( + package="wall_follower_rover", + executable="test_wall_follower", + namespace='test6', + parameters=[ + {"scan_topic": "/scan", + "drive_topic": "/drive", + "pose_topic": "/pose", + "velocity": 3., + "desired_distance": 0.72, + "side": 1, + "start_x": -7.0, + "start_y": 10.6, + "start_z": 0., + "end_x": -4.0, + "end_y": -5.0, + "name": "long_left"}], + name='test_wall_follower', + remappings=[ + ('/test6/pose', '/pose'), + ('/test6/map', '/map'), + ('/test6/base_link', '/base_link'), + ('/test6/tf', '/tf'), + ('/test6/tf_static', '/tf_static'), + ] + ) + + + ############################################################################ + ### Define wall follower node + ############################################################################ + wall_follower = Node( + package="wall_follower_rover", + executable="wall_follower", + namespace='wall_follower_ns', + parameters=[ + {"scan_topic": "/scan", + "drive_topic": "/drive", + "velocity": 1., + "desired_distance": 1., + "side": -1}], + name='wall_follower', + remappings=[ + ('/wall_follower_ns/pose', '/pose'), + ('/wall_follower_ns/map', '/map'), + ('/wall_follower_ns/base_link', '/base_link'), + ('/wall_follower_ns/tf', '/tf'), + ('/wall_follower_ns/tf_static', '/tf_static'), + ] + ) + + + ############################################################################ + ### Define commands to change parameters of the wall_follower node + ############################################################################ + setup_side2_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + setup_side2_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + setup_side2_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + + setup_side3_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '-1' + ]], + shell=True + ) + setup_side3_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '-1' + ]], + shell=True + ) + setup_side3_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '-1' + ]], + shell=True + ) + + setup_v3_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'velocity ', + '2.' + ]], + shell=True + ) + setup_v3_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'velocity ', + '2.' + ]], + shell=True + ) + setup_v3_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'velocity ', + '2.' + ]], + shell=True + ) + + setup_side4_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + setup_side4_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + setup_side4_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + + setup_side5_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '-1' + ]], + shell=True + ) + setup_side5_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '-1' + ]], + shell=True + ) + setup_side5_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '-1' + ]], + shell=True + ) + + setup_side6_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + setup_side6_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + setup_side6_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'side ', + '1' + ]], + shell=True + ) + + setup_v6_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'velocity ', + '3.' + ]], + shell=True + ) + setup_v6_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'velocity ', + '3.' + ]], + shell=True + ) + setup_v6_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'velocity ', + '3.' + ]], + shell=True + ) + + setup_d6_1 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'desired_distance ', + '0.72' + ]], + shell=True + ) + setup_d6_2 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'desired_distance ', + '0.72' + ]], + shell=True + ) + setup_d6_3 = ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' param set ', + '/wall_follower_ns/wall_follower ', + 'desired_distance ', + '0.72' + ]], + shell=True + ) + + + ############################################################################ + ### Define launch description + ############################################################################ + return LaunchDescription([ + test1, + + # Start wall follower at beginning of the test (wall follower node is only spun up once) + RegisterEventHandler( + OnProcessStart( + target_action=test1, + on_start=[ + LogInfo(msg='Test 1 started, starting wall follower'), + TimerAction( + period=1.0, + actions=[wall_follower], + ) + ] + ) + ), + + # Start test 2 after test 1 finishes + RegisterEventHandler( + OnProcessExit( + target_action=test1, + on_exit=[ + LogInfo(msg='Test 1 finished, Starting Test 2'), + test2, + setup_side2_1, + TimerAction( # Wait 1 second, then run setup_side2 again (trying to increase reliability) + period=1.0, + actions=[setup_side2_2], + ), + TimerAction( # Wait 4 seconds, then run setup_side2 again (trying to increase reliability) + period=4.0, + actions=[setup_side2_3], + ) + ] + ) + ), + + # Start test 3 after test 2 finishes + RegisterEventHandler( + OnProcessExit( + target_action=test2, + on_exit=[ + LogInfo(msg='Test 2 finished, Starting Test 3'), + test3, + setup_side3_1, + setup_v3_1, + TimerAction( # Wait 1 second, then run setup_side3 again (trying to increase reliability) + period=1.0, + actions=[setup_side3_2], + ), + TimerAction( # Wait 1 second, then run setup_v3 again (trying to increase reliability) + period=1.0, + actions=[setup_v3_2], + ), + TimerAction( # Wait 4 seconds, then run setup_side3 again (trying to increase reliability) + period=4.0, + actions=[setup_side3_3], + ), + TimerAction( # Wait 4 seconds, then run setup_v3 again (trying to increase reliability) + period=4.0, + actions=[setup_v3_3], + ) + ] + ) + ), + + # Start test 4 after test 3 finishes + RegisterEventHandler( + OnProcessExit( + target_action=test3, + on_exit=[ + LogInfo(msg='Test 3 finished, Starting Test 4'), + test4, + setup_side4_1, + TimerAction( # Wait 1 second, then run setup_side4 again (trying to increase reliability) + period=1.0, + actions=[setup_side4_2], + ), + TimerAction( # Wait 4 seconds, then run setup_side4 again (trying to increase reliability) + period=4.0, + actions=[setup_side4_3], + ) + ] + ) + ), + + # Start test 5 after test 4 finishes + RegisterEventHandler( + OnProcessExit( + target_action=test4, + on_exit=[ + LogInfo(msg='Test 4 finished, Starting Test 5'), + test5, + setup_side5_1, + TimerAction( # Wait 1 second, then run setup_side5 again (trying to increase reliability) + period=1.0, + actions=[setup_side5_2], + ), + TimerAction( # Wait 4 seconds, then run setup_side5 again (trying to increase reliability) + period=4.0, + actions=[setup_side5_3], + ) + ] + ) + ), + + # Start test 6 after test 5 finishes + RegisterEventHandler( + OnProcessExit( + target_action=test5, + on_exit=[ + LogInfo(msg='Test 5 finished, Starting Test 6'), + test6, + setup_side6_1, + setup_v6_1, + setup_d6_1, + TimerAction( # Wait 1 second, then run setup_side6 again (trying to increase reliability) + period=1.0, + actions=[setup_side6_2], + ), + TimerAction( # Wait 1 second, then run setup_v6 again (trying to increase reliability) + period=1.0, + actions=[setup_v6_2], + ), + TimerAction( # Wait 1 second, then run setup_d6 again (trying to increase reliability) + period=1.0, + actions=[setup_d6_2], + ), + TimerAction( # Wait 4 seconds, then run setup_side6 again (trying to increase reliability) + period=4.0, + actions=[setup_side6_3], + ), + TimerAction( # Wait 4 seconds, then run setup_v6 again (trying to increase reliability) + period=4.0, + actions=[setup_v6_3], + ), + TimerAction( # Wait 1 second, then run setup_d6 again (trying to increase reliability) + period=4.0, + actions=[setup_d6_3], + ) + ] + ) + ), + ]) diff --git a/wall_follower_real/launch/launch_test_sim.launch.py b/wall_follower_real/launch/launch_test_sim.launch.py new file mode 100644 index 0000000..3061fde --- /dev/null +++ b/wall_follower_real/launch/launch_test_sim.launch.py @@ -0,0 +1,64 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource +import os +from ament_index_python.packages import get_package_share_directory + + + +def generate_launch_description(): + ld = LaunchDescription() + + building_yaml = os.path.join( + get_package_share_directory('racecar_simulator'), + 'maps', + 'building_31.yaml' + ) + + map_server = Node( + package="nav2_map_server", + executable="map_server", + name='map_server', + output="screen", + parameters=[{ + "yaml_filename": building_yaml}] + ) + + lifecycle_manager = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name='lifecycle_manager', + output="screen", + parameters=[{ + "autostart": True, + "node_names": ['map_server']}] + ) + + racecar_model = IncludeLaunchDescription( + XMLLaunchDescriptionSource([os.path.join( + get_package_share_directory('racecar_simulator'), 'launch'), + '/racecar_model.launch.xml']) + ) + + config = os.path.join( + get_package_share_directory('racecar_simulator'), + 'params.yaml' + ) + + racecar_simulator = Node( + package="racecar_simulator", + executable="simulate", + name='racecar_simulator', + output="screen", + parameters=[config] + ) + + + ld.add_action(map_server) + ld.add_action(lifecycle_manager) + ld.add_action(racecar_model) + ld.add_action(racecar_simulator) + + + return ld \ No newline at end of file diff --git a/wall_follower_real/launch/safety_real.launch.py b/wall_follower_real/launch/safety_real.launch.py new file mode 100644 index 0000000..993104f --- /dev/null +++ b/wall_follower_real/launch/safety_real.launch.py @@ -0,0 +1,31 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + safety_params = os.path.join( + get_package_share_directory('safety_controller'), + 'params.yaml' + ) + return LaunchDescription([ + Node( + package='wall_follower_rover', + executable='wall_follower', + name='wall_follower', + parameters=[ + {'drive_topic': '/vesc/low_level/ackermann_cmd'}, + ] + ), + Node( + package='safety_controller', + executable='controller', + name='safety_controller', + parameters=[ + safety_params, + {'scan_topic': '/scan'}, + {'drive_topic_listen': '/vesc/low_level/ackermann_cmd'}, + {'drive_topic_publish':'/vesc/low_level/input/safety'}, + ] + ), + ]) diff --git a/wall_follower_real/launch/wall_follower.launch.xml b/wall_follower_real/launch/wall_follower.launch.xml new file mode 100644 index 0000000..b383031 --- /dev/null +++ b/wall_follower_real/launch/wall_follower.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/wall_follower_real/package.xml b/wall_follower_real/package.xml new file mode 100644 index 0000000..2a3c2f6 --- /dev/null +++ b/wall_follower_real/package.xml @@ -0,0 +1,30 @@ + + + wall_follower_rover + 1.0.0 + A wall follower for the racecar. + racecar@mit.edu + + MIT + + ament_python + + ackermann_msgs + geometry_msgs + rclpy + sensor_msgs + std_msgs + tf2_ros + visualization_msgs + + + ament_python + ackermann_msgs + geometry_msgs + rclpy + sensor_msgs + std_msgs + tf2_ros + visualization_msgs + + diff --git a/wall_follower_real/resource/wall_follower_rover b/wall_follower_real/resource/wall_follower_rover new file mode 100644 index 0000000..e69de29 diff --git a/wall_follower_real/setup.cfg b/wall_follower_real/setup.cfg new file mode 100644 index 0000000..8734527 --- /dev/null +++ b/wall_follower_real/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/wall_follower_rover +[install] +install_scripts=$base/lib/wall_follower_rover diff --git a/wall_follower_real/setup.py b/wall_follower_real/setup.py new file mode 100644 index 0000000..6f75448 --- /dev/null +++ b/wall_follower_real/setup.py @@ -0,0 +1,33 @@ +import glob +import os +from setuptools import find_packages +from setuptools import setup + +package_name = 'wall_follower_rover' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/'+package_name, ['package.xml', "wall_follower_rover/params.yaml"]), + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/wall_follower_rover/launch', glob.glob(os.path.join('launch', '*launch.xml'))), + ('share/wall_follower_rover/launch', glob.glob(os.path.join('launch', '*launch.py')))], + + install_requires=['setuptools'], + zip_safe=True, + maintainer='Sebastian', + maintainer_email='sebastianag2002@gmail.com', + description='Wall Follower ROS2 Package', + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'wall_follower = wall_follower_rover.wall_follower:main', + 'viz_example = wall_follower_rover.viz_example:main', + 'test_wall_follower = wall_follower_rover.test_wall_follower:main', + ], + }, +) diff --git a/wall_follower_real/wall_follower.gif b/wall_follower_real/wall_follower.gif new file mode 100644 index 0000000..15c6fde Binary files /dev/null and b/wall_follower_real/wall_follower.gif differ diff --git a/wall_follower_real/wall_follower_loss.png b/wall_follower_real/wall_follower_loss.png new file mode 100644 index 0000000..617167a Binary files /dev/null and b/wall_follower_real/wall_follower_loss.png differ diff --git a/wall_follower_real/wall_follower_rover/__init__.py b/wall_follower_real/wall_follower_rover/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/wall_follower_real/wall_follower_rover/np_encrypt.py b/wall_follower_real/wall_follower_rover/np_encrypt.py new file mode 100644 index 0000000..4d7a6e1 --- /dev/null +++ b/wall_follower_real/wall_follower_rover/np_encrypt.py @@ -0,0 +1,27 @@ +""" +DO NOT MODIFY THIS FILE. + +This encrypts/decrypts your numpy arrays for grading purposes. +""" + +import numpy as np + +# http://www.brianveitch.com/cryptography/generate_rsa_keys.php +public_key = 1305295067 +private_key = 3070545323 +modulus = 7366846427 + +pow_modulo = np.frompyfunc(pow, 3, 1) + + +def encode(array): + array_int = array.astype(np.float32).view(np.uint32).astype(np.uint64) + array_int_enc = pow_modulo(array_int, public_key, modulus) + return array_int_enc + + +def decode(array): + array_int = array.astype(np.uint64) + array_int_dec = pow_modulo(array_int, private_key, modulus) + array_dec = array_int_dec.astype(np.uint32).view(np.float32) + return array_dec diff --git a/wall_follower_real/wall_follower_rover/params.yaml b/wall_follower_real/wall_follower_rover/params.yaml new file mode 100644 index 0000000..0f6d753 --- /dev/null +++ b/wall_follower_real/wall_follower_rover/params.yaml @@ -0,0 +1,7 @@ +wall_follower: + ros__parameters: + scan_topic: "/scan" + drive_topic: "/drive" + side: -1 + velocity: 1.0 + desired_distance: 1. diff --git a/wall_follower_real/wall_follower_rover/test_wall_follower.py b/wall_follower_real/wall_follower_rover/test_wall_follower.py new file mode 100644 index 0000000..574991c --- /dev/null +++ b/wall_follower_real/wall_follower_rover/test_wall_follower.py @@ -0,0 +1,244 @@ +#!/usr/bin/env python3 +import numpy as np +import time as pythontime +import rclpy + +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Pose +from ackermann_msgs.msg import AckermannDriveStamped +from visualization_msgs.msg import Marker +from wall_follower_rover.np_encrypt import encode +from scipy.spatial.transform import Rotation as R +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class WallTest(Node): + def __init__(self): + super().__init__("test_wall_follower") + # Declare parameters to make them available for use + self.declare_parameter("scan_topic", "/scan") + self.declare_parameter("drive_topic", "/drive") + self.declare_parameter("pose_topic", "/pose") + + self.declare_parameter("side", 1) + self.declare_parameter("velocity", 1.0) + self.declare_parameter("desired_distance", 1.0) + self.declare_parameter("start_x", -4.0) + self.declare_parameter("start_y", -5.4) + self.declare_parameter("start_z", 0.0) + self.declare_parameter("end_x", 5.0) + self.declare_parameter("end_y", -5.0) + self.declare_parameter("name", "default") + + # Fetch constants from the ROS parameter server + self.TEST_NAME = self.get_parameter("name").get_parameter_value().string_value + self.SCAN_TOPIC = ( + self.get_parameter("scan_topic").get_parameter_value().string_value + ) + self.POSE_TOPIC = ( + self.get_parameter("pose_topic").get_parameter_value().string_value + ) + self.DRIVE_TOPIC = ( + self.get_parameter("drive_topic").get_parameter_value().string_value + ) + + self.SIDE = self.get_parameter("side").get_parameter_value().integer_value + self.VELOCITY = ( + self.get_parameter("velocity").get_parameter_value().double_value + ) + self.DESIRED_DISTANCE = ( + self.get_parameter("desired_distance").get_parameter_value().double_value + ) + self.START_x = self.get_parameter("start_x").get_parameter_value().double_value + self.START_y = self.get_parameter("start_y").get_parameter_value().double_value + self.START_z = self.get_parameter("start_z").get_parameter_value().double_value + self.END_x = self.get_parameter("end_x").get_parameter_value().double_value + self.END_y = self.get_parameter("end_y").get_parameter_value().double_value + self.NAME = self.get_parameter("name").get_parameter_value().string_value + + self.get_logger().info("Test Name %s" % (self.TEST_NAME)) + + self.max_time_per_test = 120 + self.end_threshold = 1.0 + + self.positions = [] + self.dist_to_end = np.infty + self.saves = {} + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.start_time = self.get_clock().now() + + # A publisher for navigation commands + self.pose_pub = self.create_publisher(Pose, "pose", 1) + self.drive_pub = self.create_publisher( + AckermannDriveStamped, self.DRIVE_TOPIC, 1 + ) + + # A publisher for the end position marker + self.marker_pub = self.create_publisher(Marker, "/end_position_marker", 1) + + # A subscriber to laser scans + self.create_subscription(LaserScan, self.SCAN_TOPIC, self.laser_callback, 1) + + self.START_POSE = [self.START_x, self.START_y, self.START_z] + self.END_POSE = [self.END_x, self.END_y] + + self.buffer_count = 0 + self.place_car(self.START_POSE) + + self.moved = False + + def place_car(self, pose): + p = Pose() + + p.position.x = pose[0] + p.position.y = pose[1] + + # Convert theta to a quaternion + quaternion = R.from_euler('xyz', [0, 0, pose[2]]).as_quat() + p.orientation.y = quaternion[1] + p.orientation.z = quaternion[2] + p.orientation.w = quaternion[3] + + self.pose_pub.publish(p) + pythontime.sleep(0.05) + + def publish_end_position_marker(self): + """Visualize the end position of the test""" + marker = Marker() + marker.header.frame_id = "map" + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = "end_position" + marker.id = 0 + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose.position.x = self.END_x + marker.pose.position.y = self.END_y + marker.pose.position.z = 0.0 + marker.scale.x = 0.5 + marker.scale.y = 0.5 + marker.scale.z = 0.5 + marker.color.a = 1.0 # Alpha + marker.color.r = 0.0 # Red + marker.color.g = 1.0 # Green + marker.color.b = 0.0 # Blue + + self.marker_pub.publish(marker) + + def laser_callback(self, laser_scan): + self.publish_end_position_marker() + + # Give buffer time for controller to begin working before letting the car go + if self.buffer_count < 100: + self.place_car(self.START_POSE) + self.buffer_count += 1 + if self.buffer_count == 30: + self.get_logger().info( + f"Placed Car: {self.START_POSE[0]}, {self.START_POSE[1]}" + ) + return + + from_frame_rel = "base_link" + to_frame_rel = "map" + + try: + t = self.tf_buffer.lookup_transform( + to_frame_rel, from_frame_rel, rclpy.time.Time() + ) + except TransformException as ex: + self.get_logger().info( + f"Could not transform {to_frame_rel} to {from_frame_rel}: {ex}" + ) + return + + if not self.moved: + diff = np.linalg.norm( + np.array([self.START_x, self.START_y]) + - np.array([t.transform.translation.x, t.transform.translation.y]) + ) + if 0.3 < (diff): + self.place_car(self.START_POSE) + self.get_logger().info( + f"Not at start {self.START_x-t.transform.translation.x}, {self.START_y-t.transform.translation.y}, diff {diff}" + ) + return + else: + self.moved = True + # self.get_logger().info('Moved: %s' % (self.moved)) + self.start_time = self.get_clock().now() + + ranges = np.array(laser_scan.ranges, dtype="float32") + + angles = np.linspace( + laser_scan.angle_min, laser_scan.angle_max, num=ranges.shape[0] + ) + + # Convert the ranges to Cartesian coordinates. + # Consider the robot to be facing in the positive x direction. + x = ranges * np.cos(angles) + y = ranges * np.sin(angles) + + # Filter out values that are out of range + # and values on the wrong side + valid_points = self.SIDE * y > 0 + valid_points = np.logical_and(valid_points, x < 1.5) + valid_points = np.logical_and(valid_points, x > 0.0) + + # Compute the average distance + dists = np.abs(y[valid_points]) + dist = np.sum(dists) / dists.shape[0] + # self.get_logger().info('Avg dist: %f' % (dist)) + + pos = [t.transform.translation.x, t.transform.translation.y] + + time = self.get_clock().now() - self.start_time + time_d = time.nanoseconds * 1e-9 + self.positions.append([time_d] + pos + [dist]) + self.dist_to_end = np.linalg.norm(np.array(pos) - np.array(self.END_POSE)) + # self.get_logger().info( + # f'Time: {time_d}, Max time: {self.max_time_per_test}') + + if time_d > self.max_time_per_test: + self.get_logger().error( + "\n\n\n\n\nERROR: Test timed out! Your car was not able to reach the target end position.\n\n\n\n\n" + ) + # Send a message of zero + stop = AckermannDriveStamped() + stop.drive.speed = 0.0 + stop.drive.steering_angle = 0.0 + self.drive_pub.publish(stop) + self.saves[self.TEST_NAME] = encode(np.array(self.positions)) + np.savez_compressed(self.TEST_NAME + "_log", **self.saves) + raise SystemExit + if self.dist_to_end < self.end_threshold: + self.get_logger().info( + "\n\n\n\n\nReached end of the test w/ Avg dist from wall = %f!\n\n\n\n\n" + % (dist) + ) + stop = AckermannDriveStamped() + stop.drive.speed = 0.0 + stop.drive.steering_angle = 0.0 + self.drive_pub.publish(stop) + self.saves[self.TEST_NAME] = encode(np.array(self.positions)) + np.savez_compressed(self.TEST_NAME + "_log", **self.saves) + raise SystemExit + + +def main(): + rclpy.init() + wall_follower_test = WallTest() + try: + rclpy.spin(wall_follower_test) + except SystemExit: + rclpy.logging.get_logger("Quitting").info("Done") + wall_follower_test.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/wall_follower_real/wall_follower_rover/visualization_tools.py b/wall_follower_real/wall_follower_rover/visualization_tools.py new file mode 100644 index 0000000..170f8af --- /dev/null +++ b/wall_follower_real/wall_follower_rover/visualization_tools.py @@ -0,0 +1,43 @@ +from geometry_msgs.msg import Point +from visualization_msgs.msg import Marker + + +class VisualizationTools: + + @staticmethod + def plot_line(x, y, publisher, color=(1.0, 0.0, 0.0), frame="/base_link"): + """ + Publishes the points (x, y) to publisher + so they can be visualized in rviz as + connected line segments. + Args: + x, y: The x and y values. These arrays + must be of the same length. + publisher: the publisher to publish to. The + publisher must be of type Marker from the + visualization_msgs.msg class. + color: the RGB color of the plot. + frame: the transformation frame to plot in. + """ + # Construct a line + line_strip = Marker() + line_strip.type = Marker.LINE_STRIP + line_strip.header.frame_id = frame + + # Set the size and color + line_strip.scale.x = 0.1 + line_strip.scale.y = 0.1 + line_strip.color.a = 1.0 + line_strip.color.r = color[0] + line_strip.color.g = color[1] + line_strip.color.b = color[2] + + # Fill the line with the desired values + for xi, yi in zip(x, y): + p = Point() + p.x = xi + p.y = yi + line_strip.points.append(p) + + # Publish the line + publisher.publish(line_strip) diff --git a/wall_follower_real/wall_follower_rover/viz_example.py b/wall_follower_real/wall_follower_rover/viz_example.py new file mode 100644 index 0000000..6964c1f --- /dev/null +++ b/wall_follower_real/wall_follower_rover/viz_example.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +import rclpy +import numpy as np +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from visualization_msgs.msg import Marker +from wall_follower_rover.visualization_tools import VisualizationTools + + +class LinePublisher(Node): + + # the topics to publish and subscribe to + WALL_TOPIC = "/wall" + + def __init__(self): + super().__init__("line_publisher") + + self.declare_parameter("wall_follower/scan_topic", "/scan") + self.SCAN_TOPIC = ( + self.get_parameter("wall_follower/scan_topic") + .get_parameter_value() + .string_value + ) + + # a publisher for our line marker + self.line_pub = self.create_publisher(Marker, self.WALL_TOPIC, 1) + + # a subscriber to get the laserscan data + self.create_subscription(LaserScan, self.SCAN_TOPIC, self.laser_callback, 10) + + def laser_callback(self, laser_scan): + # x and y should be points on your detected wall + # here we are just plotting a parabola as a demo + x = np.linspace(-2.0, 2.0, num=20) + y = np.square(x) + + VisualizationTools.plot_line(x, y, self.line_pub, frame="/laser") + + +def main(args=None): + rclpy.init(args=args) + line_publisher = LinePublisher() + rclpy.spin(line_publisher) + line_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/wall_follower_real/wall_follower_rover/wall_follower.py b/wall_follower_real/wall_follower_rover/wall_follower.py new file mode 100644 index 0000000..fa5cea6 --- /dev/null +++ b/wall_follower_real/wall_follower_rover/wall_follower.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +import numpy as np +import math +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from ackermann_msgs.msg import AckermannDriveStamped +from visualization_msgs.msg import Marker +from rcl_interfaces.msg import SetParametersResult + +from wall_follower_rover.visualization_tools import VisualizationTools + + +class WallFollower(Node): + + def __init__(self): + super().__init__("wall_follower") + # Declare parameters to make them available for use + # DO NOT MODIFY THIS! + self.declare_parameter("scan_topic", "/scan") + self.declare_parameter("drive_topic", "/drive") + self.declare_parameter("side", 1) + self.declare_parameter("velocity", 1.0) + self.declare_parameter("desired_distance", 1.0) + + # Fetch constants from the ROS parameter server + # DO NOT MODIFY THIS! This is necessary for the tests to be able to test varying parameters! + self.SCAN_TOPIC = self.get_parameter('scan_topic').get_parameter_value().string_value + self.DRIVE_TOPIC = self.get_parameter('drive_topic').get_parameter_value().string_value + self.SIDE = self.get_parameter('side').get_parameter_value().integer_value + self.VELOCITY = self.get_parameter('velocity').get_parameter_value().double_value + self.DESIRED_DISTANCE = self.get_parameter('desired_distance').get_parameter_value().double_value + + # for PID + self.PREV_ERROR = 0 + self.TICK_TIME = 0.02 + # This activates the parameters_callback function so that the tests are able + # to change the parameters during testing. + # DO NOT MODIFY THIS! + self.add_on_set_parameters_callback(self.parameters_callback) + + # TODO: Initialize your publishers and subscribers here + self.drive_commands = self.create_publisher( + AckermannDriveStamped, + self.DRIVE_TOPIC, + 10) + self.lidar_data = self.create_subscription( + LaserScan, + self.SCAN_TOPIC, + self.listener_callback, + 10) + self.lidar_data + + # TODO: Write your callback functions here + def listener_callback(self, lidar_msg): + """ + Filters out ranges to points relevant to the side and position + of the car. Uses a PID controller to calculate the necessary steering angle. + + :param lidar_msg: A point cloud in LaserScan form. + """ + + a_min, a_max, a_incr = lidar_msg.angle_min, lidar_msg.angle_max, lidar_msg.angle_increment + A = np.array(lidar_msg.ranges) # ranges array + a = a_min + np.arange(len(A), dtype=np.float32) * a_incr # angles array + + if self.SIDE == 1: + a_min, a_max = -math.pi/12, math.pi/2 + else: + a_min, a_max = -math.pi/2, math.pi/12 + + valid_r = (0.2 <= A) & (A <= 5) + valid_a = (a_min <= a) & (a <= a_max) + mask = valid_r & valid_a + + if not np.any(mask): + self.publish_msg(0.0) + return + + A = A[mask] + a = a[mask] + + x = A * np.cos(a) # convert from polar to cartesian + y = A * np.sin(a) + + m, b = np.polyfit(x,y,1) + line_angle = math.atan(m) + distance = -b / math.sqrt(m**2 + 1) # fit OLS + + e = (-self.SIDE * self.DESIRED_DISTANCE) - distance + de = e - self.PREV_ERROR + prop = 2 * e + deriv = 0.2 * de/self.TICK_TIME + + self.PREV_ERROR = e + steering_angle = prop + deriv + 2 * line_angle + + self.publish_msg(steering_angle) + + def publish_msg(self, steering_angle): + """ + Publishes a AckerMannDrive message. + + :param steering_angle: the steering angle for the drive message. + """ + msg = AckermannDriveStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "base_link" + msg.drive.steering_angle = steering_angle + msg.drive.speed = self.VELOCITY + + self.drive_commands.publish(msg) + + def parameters_callback(self, params): + """ + DO NOT MODIFY THIS CALLBACK FUNCTION! + + This is used by the test cases to modify the parameters during testing. + It's called whenever a parameter is set via 'ros2 param set'. + """ + for param in params: + if param.name == 'side': + self.SIDE = param.value + self.get_logger().info(f"Updated side to {self.SIDE}") + elif param.name == 'velocity': + self.VELOCITY = param.value + self.get_logger().info(f"Updated velocity to {self.VELOCITY}") + elif param.name == 'desired_distance': + self.DESIRED_DISTANCE = param.value + self.get_logger().info(f"Updated desired_distance to {self.DESIRED_DISTANCE}") + return SetParametersResult(successful=True) + + +def main(): + rclpy.init() + wall_follower = WallFollower() + rclpy.spin(wall_follower) + wall_follower.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/wall_follower_real/wall_follower_score.png b/wall_follower_real/wall_follower_score.png new file mode 100644 index 0000000..4476f53 Binary files /dev/null and b/wall_follower_real/wall_follower_score.png differ