90/waypoints planning (#143)

* pull in waypoints path planner from 132

* add launch

* edit meta

* oops added control instead of pp

* finish launch, works

* rm path.csv

* modify readme

* add comment for mod sub

* remove disable

docs/README.md

@@ -38,9 +38,9 @@ In addition, you will need to install docker and docker compose. Please refer to
 To build the chrono image, you'll need to download the OptiX 7.7 build script from NVIDIA's website and place it in [`docker/data`](./../docker/data). You can find the download link [here](https://developer.nvidia.com/designworks/optix/download). See the [FAQs](./misc/faq.md#optix-install) for more details.
 
 #### Download Waypoints Data Files
-To run the waypoints-based path following demo, we need to download some waypoints paths as reference trajectories into [`sim/data`](./../sim/data). You can use the following command to download the data files:
+To run the waypoints-based path following demo, we need to download some waypoints paths as reference trajectories into [`sim/data`](./../sim/data), as well as into the [`workspace/src/path_planning/waypoints_path_planner/waypoints_path_planner`](../workspace/src/path_planning/waypoints_path_planner/waypoints_path_planner/) folder. You can use the following command to download the data files:
 ```bash
-cd <path_to_autonomy_research_testbed>/sim/data && gdown --folder 1as3pPYlC0m9LcRJuuVOzuAOYPMlosMfY
+cd <path_to_autonomy_research_testbed_sim_or_path_planner_folder> && gdown --folder 1as3pPYlC0m9LcRJuuVOzuAOYPMlosMfY
 ```
 
 ### Start up vnc

workspace/src/common/launch/art_planning_launch/launch/art_planning.launch.py

@@ -13,7 +13,9 @@ def generate_launch_description():
     # ---------------
 
     IncludeLaunchDescriptionWithCondition(
-        ld, "art_planning_launch", "centerline_path_planner"
+        ld, "art_planning_launch", "centerline_objects_path_planner"
+    )
+    IncludeLaunchDescriptionWithCondition(
+        ld, "art_planning_launch", "waypoints_path_planner"
     )
-
     return ld

workspace/src/common/launch/art_planning_launch/launch/waypoints_path_planner.launch.py

@@ -0,0 +1,78 @@
+#
+# BSD 3-Clause License
+#
+# Copyright (c) 2022 University of Wisconsin - Madison
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice, this
+#   list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+#
+# * Neither the name of the copyright holder nor the names of its
+#   contributors may be used to endorse or promote products derived from
+#   this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.#
+
+# ros imports
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+# internal imports
+from launch_utils import AddLaunchArgument, GetLaunchArgument
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    # ----------------
+    # Launch Arguments
+    # ----------------
+
+    AddLaunchArgument(ld, "art_planning/input/vehicle_state", "/vehicle/filtered_state")
+    AddLaunchArgument(ld, "art_planning/output/error_state", "/vehicle/error_state")
+    AddLaunchArgument(ld, "vis", "False")
+    AddLaunchArgument(ld, "lookahead", ".75")
+
+    # -----
+    # Nodes
+    # -----
+
+    node = Node(
+        package="waypoints_path_planner",
+        executable="waypoints_path_planner",
+        name="waypoints_path_planner",
+        remappings=[
+            (
+                "~/input/vehicle_state",
+                GetLaunchArgument("art_planning/input/vehicle_state"),
+            ),
+            (
+                "~/output/error_state",
+                GetLaunchArgument("art_planning/output/error_state"),
+            ),
+        ],
+        parameters=[
+            {"vis": GetLaunchArgument("vis")},
+            {"lookahead": GetLaunchArgument("lookahead")},
+            {"use_sim_time": GetLaunchArgument("use_sim_time")},
+        ],
+    )
+    ld.add_action(node)
+
+    return ld

workspace/src/common/meta/art_dev_meta/package.xml

@@ -13,6 +13,7 @@
   <exec_depend>cone_detector</exec_depend>
   <exec_depend>ground_truth</exec_depend>
   <exec_depend>centerline_objects_path_planner</exec_depend>
+  <exec_depend>waypoints_path_planner</exec_depend>
   <exec_depend>pid_lateral_controller</exec_depend>
   <exec_depend>chrono_ros_interfaces</exec_depend>
 </package>

workspace/src/common/meta/art_oak_meta/package.xml

@@ -9,10 +9,11 @@
 
   <exec_depend>art_common_meta</exec_depend>
 
-  <exec_depend>art_launch</exec_depend>
-  <exec_depend>localization_py</exec_depend>
-  <exec_depend>path_planning</exec_depend>
-  <exec_depend>control</exec_depend>
+  <exec_depend>art_oak_launch</exec_depend>
+  <exec_depend>cone_detector</exec_depend>
+  <exec_depend>ground_truth</exec_depend>
+  <exec_depend>waypoints_path_planner</exec_depend>
+  <exec_depend>pid_lateral_controller</exec_depend>
   <exec_depend>chrono_ros_interfaces</exec_depend>
   <!-- add sensing -->
 </package>

workspace/src/path_planning/waypoints_path_planner/package.xml

@@ -0,0 +1,25 @@
+<?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>waypoints_path_planner</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">TODO</maintainer>
+  <license>TODO: License declaration</license>
+
+  <exec_depend>rclpy</exec_depend>
+
+  <exec_depend>art_msgs</exec_depend>
+  <exec_depend>art_perception_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-matplotlib</exec_depend>
+  <exec_depend version_gte="1.8">python3-scipy</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

workspace/src/path_planning/waypoints_path_planner/setup.cfg

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

workspace/src/path_planning/waypoints_path_planner/setup.py

@@ -0,0 +1,58 @@
+#
+# BSD 3-Clause License
+#
+# Copyright (c) 2022 University of Wisconsin - Madison
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice, this
+#   list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+#
+# * Neither the name of the copyright holder nor the names of its
+#   contributors may be used to endorse or promote products derived from
+#   this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.#
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = "waypoints_path_planner"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "data/"), glob("data/*")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="TODO:",
+    maintainer_email="TODO:",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "waypoints_path_planner = waypoints_path_planner.waypoints_path_planner:main"
+        ],
+    },
+)

workspace/src/path_planning/waypoints_path_planner/waypoints_path_planner/__init__.py

@@ -0,0 +1,30 @@
+#
+# BSD 3-Clause License
+#
+# Copyright (c) 2022 University of Wisconsin - Madison
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice, this
+#   list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+#
+# * Neither the name of the copyright holder nor the names of its
+#   contributors may be used to endorse or promote products derived from
+#   this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.#