Lidar Drag Race Lane Following

compile_commands.json

@@ -0,0 +1 @@
+build/compile_commands.json

install_dependencies.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+# The current directory
+CURR_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+# Installs all required dependencies to build this repo
+rosdep install --from-paths $CURR_DIR --ignore-src --rosdistro=ROSDISTRO

src/drag_race/CMakeLists.txt

@@ -0,0 +1,90 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(drag_race)
+
+# This exports the compile commands so people using Vim with YouCompleteMe
+# can have easy autocompletion
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+add_definitions(--std=c++14)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        )
+find_package(sb_utils REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# TODO: Wait. Do we need this line if we're not "exporting" the headers from the package
+catkin_package(
+  INCLUDE_DIRS include
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+    ${sb_utils_INCLUDE_DIRS}
+    ${catkin_INCLUDE_DIRS}
+    ./include
+    )
+
+## Declare a C++ executable
+# add_executable(${PROJECT_NAME}_node src/drag_race_node.cpp)
+
+# TODO: Perhaps rename to something like `drag_race_controller_node`
+add_executable(drag_race_node
+        src/drag_race_node.cpp
+        src/DragRaceNode.cpp
+        src/DragRaceController.cpp
+        src/LidarObstacleManager.cpp
+        src/LidarObstacle.cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+target_link_libraries(drag_race_node
+        ${catkin_LIBRARIES}
+        ${sb_utils_LIBRARIES}
+        )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_drag_race.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+if (CATKIN_ENABLE_TESTING)
+
+    # Adding gtests to the package
+    catkin_add_gtest(lidar-obstacle-manager-test
+        test/lidar-obstacle-manager-test.cpp
+        src/LidarObstacleManager.cpp
+        src/LidarObstacle.cpp)
+    target_link_libraries(lidar-obstacle-manager-test ${catkin_LIBRARIES})
+
+    catkin_add_gtest(lidar-obstacle-test 
+        test/lidar-obstacle-test.cpp
+        src/LidarObstacle)
+    target_link_libraries(lidar-obstacle-test ${catkin_LIBRARIES})
+
+    catkin_add_gtest(drag-race-controller-test
+            test/drag-race-controller-test.cpp
+            src/DragRaceController.cpp)
+    target_link_libraries(drag-race-controller-test ${catkin_LIBRARIES})
+
+    # TODO: Maybe some rostests?
+
+endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/drag_race/include/DragRaceController.h

@@ -0,0 +1,77 @@
+/*
+ * Created By: Robyn Castro
+ * Created On: July 9th, 2017
+ * Description: Given a line and a set of parameters, determines
+ *              how the car should move in the drag race.
+ */
+
+#ifndef DRAG_RACE_CONTROLLER_H
+#define DRAG_RACE_CONTROLLER_H
+
+// ROS Includes
+#include <geometry_msgs/Twist.h>
+
+// SB Includes
+#include <LidarObstacleManager.h>
+
+class DragRaceController {
+public:
+    /**
+     * Empty Constructor
+     */
+    DragRaceController();
+
+    /**
+     * Initializes parameters of the robot.
+     *
+     * @param targetDistance
+     * @param lineToTheRight
+     * @param theta_scaling_multiplier
+     * @param angular_speed_multiplier
+     * @param linear_speed_multiplier
+     * @param angular_vel_cap
+     * @param linear_vel_cap
+     */
+    DragRaceController(double targetDistance,
+                       bool lineToTheRight, double theta_scaling_multiplier,
+                       double angular_speed_multiplier, double linear_speed_multiplier,
+                       double angular_vel_cap, double linear_vel_cap);
+
+    /**
+     * Determines the optimal movement to stay within target distance of
+     * the given line.
+     *
+     * @param longestConeLine
+     * @return the optimal angular and linear acceleration.
+     */
+    geometry_msgs::Twist determineDesiredMotion(LineOfBestFit longestConeLine, bool no_line_on_expected_side);
+
+private:
+
+    /**
+     * Finds the minimum distance from given line and the origin.
+     *
+     * @param line
+     * @return the minimum distance from given line and the origin.
+     */
+    static double determineDistanceFromLine(LineOfBestFit line);
+
+    // How far from the target line the robot should be
+    double target_distance;
+
+    // Where the target line is
+    bool line_to_the_right;
+
+    // Velocity limits
+    double angular_vel_cap;
+    double linear_vel_cap;
+
+    // Scaling
+    double theta_scaling_multiplier;
+    double angular_speed_multiplier;
+    double linear_speed_multiplier;
+
+};
+
+
+#endif // DRAG_RACE_CONTROLLER_H

src/drag_race/include/DragRaceNode.h

@@ -0,0 +1,98 @@
+/*
+ * Created By: Gareth Ellis
+ * Created On: July 16th, 2016
+ * Description: An example node that subscribes to a topic publishing strings,
+ *              and re-publishes everything it receives to another topic with
+ *              a "!" at the end
+ */
+
+#ifndef DRAG_RACE_NODE_DRAG_RACE_H
+#define DRAG_RACE_NODE_DRAG_RACE_H
+
+// STD Includes
+#include <iostream>
+#include <vector>
+
+// ROS Includes
+// TODO: Sort me for neatness
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <sensor_msgs/LaserScan.h>
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Bool.h>
+
+// SB Includes
+#include <sb_utils.h>
+#include <LidarObstacleManager.h>
+#include <DragRaceController.h>
+
+class DragRaceNode {
+public:
+    DragRaceNode(int argc, char **argv, std::string node_name);
+
+private:
+    // TODO: Doc comment
+    void scanCallBack(const sensor_msgs::LaserScan::ConstPtr &scan);
+
+    void greenLightCallBack(const std_msgs::Bool &green_light_detected);
+
+    // Manages obstacles, including the cones and wall
+    LidarObstacleManager obstacle_manager;
+
+    // Manages line handling and movement
+    DragRaceController drag_race_controller;
+
+
+    // How far lines can be before being considered invalid
+    double max_distance_from_robot_accepted;
+
+    // How far from the target line the robot should be
+    double target_distance;
+
+    // Where the target line is
+    bool line_to_the_right;
+
+    // Velocity limits
+    double angular_vel_cap;
+    double linear_vel_cap;
+
+    // Scaling
+    double theta_scaling_multiplier;
+    double angular_speed_multiplier;
+    double linear_speed_multiplier;
+
+    // Traffic light detection
+    int minimum_green_recognised_count;
+    int green_count_recognised;
+
+    // End zone collision detection
+    // TODO: Could make all these local variables?
+    int incoming_obstacle_ticks;
+    int obstacle_ticks_threshold;
+    double collision_distance;
+    double front_angle;
+    double side_angle_max;
+    double side_angle_min;
+    double region_fill_percentage;
+    bool front_collision_only;
+
+    // Signals that we're at the end of the course
+    bool end_of_course;
+
+    // Subscribes to the LaserScan
+    ros::Subscriber scan_subscriber;
+    // Subscribes to traffic light detection
+    ros::Subscriber traffic_light_subscriber;
+
+    // Publishes Twist messages to control the robot
+    ros::Publisher twist_publisher;
+    // Publishes the obstacles so we can see them in RViz
+    ros::Publisher cone_debug_publisher;
+    // Publishes the cone lines so we can see them in RViz
+    ros::Publisher cone_line_debug_publisher;
+    // Publishes the cone line we're using to determine the twist message
+    // so we can see it in RViz
+    ros::Publisher best_line_debug_publisher;
+};
+
+#endif //DRAG_RACE_NODE_DRAG_RACE_H