waypoints (#93)

* precommit

* implement art_planning_waypoints

workspace/src/common/launch/art_oak_launch/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.5)
+project(art_oak_launch)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

workspace/src/common/launch/art_oak_launch/launch/art_oak.launch.py

@@ -0,0 +1,123 @@
+#
+# 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.actions import SetLaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import ComposableNodeContainer
+
+# internal imports
+from launch_utils import (
+    IncludeLaunchDescriptionWithCondition,
+    GetLaunchArgument,
+    AddLaunchArgument,
+    SetLaunchArgument,
+)
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    # ----------------
+    # Launch Arguments
+    # ----------------
+
+    AddLaunchArgument(ld, "use_sim", "False")
+    AddLaunchArgument(ld, "use_sim_time", "False")
+    SetLaunchArgument(
+        ld, "use_sim_time", "True", condition=IfCondition(GetLaunchArgument("use_sim"))
+    )
+
+    AddLaunchArgument(ld, "container", "")
+    container_name = AddLaunchArgument(ld, "container_name", "art_container")
+
+    SetLaunchArgument(
+        ld,
+        "disable_art_sensing",
+        "True",
+        condition=IfCondition(GetLaunchArgument("use_sim")),
+    )
+    SetLaunchArgument(
+        ld,
+        "disable_art_vehicle",
+        "True",
+        condition=IfCondition(GetLaunchArgument("use_sim")),
+    )
+    SetLaunchArgument(
+        ld,
+        "disable_art_simulation",
+        "True",
+        condition=UnlessCondition(GetLaunchArgument("use_sim")),
+    )
+
+    # -------------
+    # Composability
+    # -------------
+
+    # If composability is desired, all included launch descriptions should attach to this container and use intraprocess communication
+
+    use_composability = IfCondition(AddLaunchArgument(ld, "use_composability", "False"))
+
+    # If a container name is not provided,
+    # set the name of the container launched above for image_proc nodes
+    set_container_name = SetLaunchConfiguration(
+        condition=use_composability, name="container", value=container_name
+    )
+    ld.add_action(set_container_name)
+
+    container = ComposableNodeContainer(
+        name=container_name,
+        namespace="",
+        package="rclcpp_components",
+        executable="component_container",
+        composable_node_descriptions=[],
+        condition=use_composability,
+        output="screen",
+    )
+    ld.add_action(container)
+
+    # ---------------
+    # Launch Includes
+    # ---------------
+
+    IncludeLaunchDescriptionWithCondition(
+        ld, "art_localization_launch", "art_localization"
+    )
+    IncludeLaunchDescriptionWithCondition(
+        ld, "art_planning_waypoints_launch", "art_planning_waypoints"
+    )
+    IncludeLaunchDescriptionWithCondition(ld, "art_control_launch", "art_control")
+    IncludeLaunchDescriptionWithCondition(ld, "art_sensing_launch", "art_sensing")
+    IncludeLaunchDescriptionWithCondition(ld, "art_vehicle_launch", "art_vehicle")
+    # IncludeLaunchDescriptionWithCondition(ld, "art_simulation_launch", "art_simulation")
+
+    return ld

workspace/src/common/launch/art_oak_launch/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>art_oak_launch</name>
+  <version>0.1.0</version>
+  <description>The art_oak_launch package</description>
+
+  <maintainer email="[email protected]">TODO</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <exec_depend>art_localization_launch</exec_depend>
+  <exec_depend>art_control_launch</exec_depend>
+  <exec_depend>art_planning_waypoints_launch</exec_depend>
+  <exec_depend>art_sensing_launch</exec_depend>
+  <exec_depend>art_vehicle_launch</exec_depend>
+  <exec_depend>launch_utils</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

workspace/src/common/launch/art_planning_waypoints_launch/art_planning_launch/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.5)
+project(art_planning_waypoints_launch)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

workspace/src/common/launch/art_planning_waypoints_launch/art_planning_launch/launch/art_planning_waypoints.launch.py

@@ -0,0 +1,81 @@
+#
+# 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_waypoints/input/vehicle_state", "/vehicle/state"
+    )
+    AddLaunchArgument(
+        ld, "art_planning_waypoints/output/error_state", "/path_planning/error_state"
+    )
+
+    AddLaunchArgument(ld, "lookahead", ".75")
+
+    # -----
+    # Nodes
+    # -----
+
+    node = Node(
+        package="path_planning_waypoints",
+        executable="path_planning_waypoints",
+        name="path_planning_waypoints",
+        remappings=[
+            (
+                "~/input/vehicle_state",
+                GetLaunchArgument("art_planning_waypoints/input/vehicle_state"),
+            ),
+            (
+                "~/output/error_state",
+                GetLaunchArgument("art_planning_waypoints/output/error_state"),
+            ),
+        ],
+        parameters=[
+            {"lookahead": GetLaunchArgument("lookahead")},
+            {"use_sim_time": GetLaunchArgument("use_sim_time")},
+        ],
+    )
+    ld.add_action(node)
+
+    return ld

workspace/src/common/launch/art_planning_waypoints_launch/art_planning_launch/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>art_planning_waypoints_launch</name>
+  <version>0.1.0</version>
+  <description>The art_planning_waypoints_launch package</description>
+
+  <maintainer email="[email protected]">TODO</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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

@@ -9,9 +9,9 @@
 
   <exec_depend>art_common_meta</exec_depend>
 
-  <exec_depend>art_launch</exec_depend>
+  <exec_depend>art_oak_launch</exec_depend>
   <exec_depend>localization_py</exec_depend>
-  <exec_depend>path_planning</exec_depend>
+  <exec_depend>path_planning_waypoints</exec_depend>
   <exec_depend>control</exec_depend>
   <exec_depend>chrono_ros_interfaces</exec_depend>
   <!-- add sensing -->

workspace/src/path_planning_waypoints/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>path_planning_waypoints</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">asher</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>python3-scipy</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

workspace/src/path_planning_waypoints/path_planning_waypoints/__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.#