diff --git a/.gitignore b/.gitignore
index 6a39dbd..c54510b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,7 @@
.vscode/*
.history/
+
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
diff --git a/ff_path_planning/README.md b/ff_path_planning/README.md
new file mode 100644
index 0000000..1cf7b6d
--- /dev/null
+++ b/ff_path_planning/README.md
@@ -0,0 +1,128 @@
+# Freeflyer Path Planning
+
+# Introduction
+
+Please read both the Quickstart and Caveats.
+
+## Quickstart
+
+To run the path planning node
+```bash
+$ ros2 run ff_path_planning path_planning
+```
+
+To run the example client call
+```bash
+$ ros2 run ff_path_planning examples:service_call
+```
+
+## Caveats
+
+* this node depends on [https://github.com/StanfordASL/pmpc](https://github.com/StanfordASL/pmpc) so that package must be correctly installed
+* the `pmpc` installation can take up to 20 min!
+* the `pmpc` depends on Julia, so:
+ * installation might take up 10 - 30 min
+ * first 1-2 runs (`ros2 run ...`) might take 1 - 5 min
+ * subsequent runs should run much faster
+ * *note*: the path planning module supports hot-reloading of dynamics and cost, no need to restart it
+
+---
+
+# Path Planning Service
+
+This node defines the following service
+```
+string dynamics
+string cost "default_cost"
+float64[] x0
+float64 t0
+float64 dt 0.25
+int64 horizon 20
+int64 xdim -1
+int64 udim -1
+int64 max_it 20
+string params_json
+---
+float64[] times
+float64[] states
+float64[] controls
+float64[] feedback
+```
+
+---
+
+# Defining Custom Dynamics and Costs
+
+The path planning node **automatically discovers** new dynamics and costs and
+supports hot-reloading of those when you edit the code. Start the path planning
+node once and develop as much as you want!
+
+Both the dynamics and cost function accept an optional `params` argument, a
+JSON-serializable object (e.g., a dictionary of values), which can be sent with
+every request via a JSON-string `params_json`.
+
+## Dynamics
+
+Place a custom Python file in `./ff_path_planning/dynamics`. The name of the file (minus `.py`) is the string with which you can reference your dynamics.
+
+*Example: `./ff_path_planning/dynamics/example_dynamics.py` becomes `dynamics = example_dynamics`.*
+
+The file
+* must define a `f_fx_fu_fn(x, u, params=None)` function, which **batch** evaluates the
+ * `xp`, the next state, `shape = [N, xdim]`
+ * `fx`, the state linearization, `shape = [N, xdim, xdim]`
+ * `fu`, the control linearization, `shape = [N, xdim, udim]`
+* *optionally* can define `xdim` or `XDIM` global variable
+* *optionally* can define `udim` or `UDIM` global variable
+
+Argument `params` refers to the optional problem parameters provided every service call is `params_json`.
+
+```python
+# ./ff_path_planning/dynamics/example_dynamics.py
+XDIM, UDIM = 2, 1
+
+def f_fx_fu_fn(x, u, params=None):
+ ...
+```
+
+## Cost
+
+Place a custom Python file in `./ff_path_planning/costs`. The name of the file (minus `.py`) is the string with which you can reference your cost.
+
+*Example: `./ff_path_planning/costs/example_cost.py` becomes `cost = example_cost`.*
+
+The file
+* must define a `cost(N=None, xdim=None, udim=None, params=None)` function that defines cost parameters as per [`pmpc`](https://github.com/StanfordASL/pmpc) and returns a dictionary
+
+```python
+# ./ff_path_planning/costs/example_costs.py
+def cost(N=None, xdim=None, udim=None, params=None):
+ ...
+ return dict(...)
+```
+
+---
+
+# Getting Started
+
+## How to start developing?
+
+Copy and modify `./ff_path_planning/examples/path_planning.py`
+
+Take a look at `//freeflyer2/freeflyer/launch/sim_path_plnner.launch.py` which can be launched via
+
+```bash
+$ # launch path planning node separately to gain cleaner output
+$ ros2 run ff_path_planning path_planning
+$ # then, launch the simulation example
+$ ros2 launch freeflyer sim_path_planner.launch.py
+```
+
+## Examples
+
+* a full path planning example: `./ff_path_planning/examples/path_planning.py`
+ * run via `ros2 run ff_path_planning examples:path_planning`
+* calling the path planning service: `./ff_path_planning/examples/service_call.py`
+ * run via `ros2 run ff_path_planning examples:service_call`
+* setting a static goal for the path-following controller: `./ff_path_planning/examples/setting_goal.py`
+ * run via `ros2 run ff_path_planning examples:setting_goal`
diff --git a/ff_path_planning/ff_path_planning/__init__.py b/ff_path_planning/ff_path_planning/__init__.py
new file mode 100644
index 0000000..89a3ecb
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/__init__.py
@@ -0,0 +1,26 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+from .path_planning import main
+
+if __name__ == "__main__":
+ main()
diff --git a/ff_path_planning/ff_path_planning/costs/__init__.py b/ff_path_planning/ff_path_planning/costs/__init__.py
new file mode 100644
index 0000000..23995af
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/costs/__init__.py
@@ -0,0 +1,21 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
diff --git a/ff_path_planning/ff_path_planning/costs/default_cost.py b/ff_path_planning/ff_path_planning/costs/default_cost.py
new file mode 100644
index 0000000..694b3d6
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/costs/default_cost.py
@@ -0,0 +1,29 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+from pmpc import Problem
+
+
+def cost(N: int, xdim: int, udim: int, params=None):
+ # let's just use the default cost builder from pmpc
+ p = Problem(N=N, xdim=xdim, udim=udim)
+ return dict(Q=p.Q, R=p.R, X_ref=p.X_ref, U_ref=p.U_ref)
diff --git a/ff_path_planning/ff_path_planning/costs/my_cost.py b/ff_path_planning/ff_path_planning/costs/my_cost.py
new file mode 100644
index 0000000..04c9525
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/costs/my_cost.py
@@ -0,0 +1,38 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+from pmpc import Problem
+import numpy as np
+
+
+def cost(N: int, xdim: int, udim: int, params=None):
+ # let's just use the default cost builder from pmpc
+ p = Problem(N=N, xdim=xdim, udim=udim)
+ if "Q" in params:
+ p.Q = np.array(params["Q"])
+ if "R" in params:
+ p.R = np.array(params["R"])
+ if "X_ref" in params:
+ p.X_ref = np.array(params["X_ref"])
+ if "U_ref" in params:
+ p.U_ref = np.array(params["U_ref"])
+ return dict(Q=p.Q, R=p.R, X_ref=p.X_ref, U_ref=p.U_ref)
diff --git a/ff_path_planning/ff_path_planning/dynamics/__init__.py b/ff_path_planning/ff_path_planning/dynamics/__init__.py
new file mode 100644
index 0000000..23995af
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/dynamics/__init__.py
@@ -0,0 +1,21 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
diff --git a/ff_path_planning/ff_path_planning/dynamics/single_integrator.py b/ff_path_planning/ff_path_planning/dynamics/single_integrator.py
new file mode 100644
index 0000000..44680b9
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/dynamics/single_integrator.py
@@ -0,0 +1,36 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import numpy as np
+
+A, B = np.array([[1.0]]), np.array([[1.0]])
+XDIM, UDIM = A.shape[-1], B.shape[-1]
+
+
+def f_fx_fu_fn(x, u, params=None):
+ params = dict() if params is None else params
+ Bp = B * params["dt"] if "dt" in params else B
+ assert x.shape[-1] == 1 and u.shape[-1] == 1
+ xp = (A @ x[..., None])[..., 0] + (Bp @ u[..., None])[..., 0]
+ fx = np.tile(A, x.shape[:-1] + (1, 1))
+ fu = np.tile(Bp, x.shape[:-1] + (1, 1))
+ return xp, fx, fu
diff --git a/ff_path_planning/ff_path_planning/dynamics/single_integrator_2D.py b/ff_path_planning/ff_path_planning/dynamics/single_integrator_2D.py
new file mode 100644
index 0000000..7bf089b
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/dynamics/single_integrator_2D.py
@@ -0,0 +1,36 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import numpy as np
+
+A, B = np.eye(2), np.eye(2)
+XDIM, UDIM = A.shape[-1], B.shape[-1]
+
+
+def f_fx_fu_fn(x, u, params=None):
+ params = dict() if params is None else params
+ Bp = B * params["dt"] if "dt" in params else B
+ assert x.shape[-1] == XDIM and u.shape[-1] == UDIM
+ xp = (A @ x[..., None])[..., 0] + (Bp @ u[..., None])[..., 0]
+ fx = np.tile(A, x.shape[:-1] + (1, 1))
+ fu = np.tile(Bp, x.shape[:-1] + (1, 1))
+ return xp, fx, fu
diff --git a/ff_path_planning/ff_path_planning/examples/__init__.py b/ff_path_planning/ff_path_planning/examples/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/ff_path_planning/ff_path_planning/examples/path_planning.py b/ff_path_planning/ff_path_planning/examples/path_planning.py
new file mode 100644
index 0000000..67869f7
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/examples/path_planning.py
@@ -0,0 +1,187 @@
+#!/usr/bin/env python3
+
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import json
+
+import rclpy
+from rclpy.time import Time, Duration
+from rclpy.node import Node
+from ff_srvs.srv import PathPlan
+from ff_msgs.msg import FreeFlyerStateStamped, FreeFlyerState, Twist2D, Pose2D as FF_Pose2D
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+
+import numpy as np
+from scipy.interpolate import interp1d
+
+####################################################################################################
+
+
+def to_ros_array(x: np.ndarray):
+ return x.reshape(-1).tolist()
+
+
+####################################################################################################
+
+
+class NavigationNode(Node):
+ def __init__(self):
+ """A path planning service node based on the `pmpc` library."""
+ super().__init__("simple_goal")
+ self.ctrl_state_pub = self.create_publisher(FreeFlyerStateStamped, "ctrl/state", 10)
+ self.state_sub = self.create_subscription(
+ FreeFlyerStateStamped, "gt/state", self.state_cb, 10
+ )
+ self.state, self.goal_pose = None, None
+ self.path_plan_client = self.create_client(PathPlan, "/path_planning")
+ while not self.path_plan_client.wait_for_service(timeout_sec=1.0):
+ self.get_logger().info("service not available, waiting again...")
+
+ self.marker_pub = self.create_publisher(Path, "path_marker", 10)
+
+ self.computing_plan, self.plan = False, None
+ self.plan_futures = []
+ self.create_timer(1.0, self.recompute_plan_cb)
+ self.create_timer(1.0, self.publish_goal_cb)
+
+ self.get_logger().info("Navigation node is up")
+
+ pub = self.create_publisher(FreeFlyerStateStamped, "state_init", 10)
+ msg = FreeFlyerStateStamped()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.state = FreeFlyerState(twist=Twist2D(), pose=FF_Pose2D())
+ self.get_logger().info("Publishing initial state")
+ pub.publish(msg)
+ self.create_timer(1e-2, self.loop)
+
+ def show_path(self) -> None:
+ """Publish the path as a path marker."""
+ if self.plan is None:
+ return
+ X = self.plan["X"]
+ marker = Path()
+ marker.header.frame_id = "map"
+ marker.header.stamp = self.get_clock().now().to_msg()
+ marker.poses = []
+ t0 = self.plan["t"][0]
+ for i in range(X.shape[0]):
+ pose = PoseStamped()
+ pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = X[i, 0], X[i, 1], 0.5
+ pose.header.frame_id = marker.header.frame_id
+
+ time_now = Time.from_msg(marker.header.stamp)
+ dt = Duration(seconds=self.plan["t"][i] - t0)
+ pose.header.stamp = (time_now + dt).to_msg()
+
+ marker.poses.append(pose)
+ self.marker_pub.publish(marker)
+
+ def loop(self) -> None:
+ """Pull plans if they are available."""
+
+ # attempt to parse returned planning service calls
+ last_future, incomplete_futures = None, []
+ for i in range(len(self.plan_futures)):
+ last_future = self.plan_futures[i]
+ if not last_future.done():
+ incomplete_futures.append(last_future)
+ self.plan_futures = incomplete_futures
+ if last_future is None:
+ return
+
+ # check if the service call is not None
+ result = last_future.result()
+ if result is None:
+ return
+
+ # parse the plan from the service call
+ ts = np.array(result.times)
+ N = len(ts) - 1
+ X = np.array(result.states).reshape((N + 1, -1))
+ U = np.array(result.controls).reshape((N, -1))
+ L = np.array(result.feedback).reshape((N, X.shape[-1], U.shape[-1]))
+ self.plan = dict(t=ts, X=X, U=U, L=L)
+ self.show_path()
+
+ def make_goal_pose(self, x: float, y: float) -> FreeFlyerStateStamped:
+ """Create a goal pose from the current state, with the given x and y coordinates."""
+ th = self.state.state.pose.theta
+ goal_pose = FreeFlyerStateStamped()
+ goal_pose.header.stamp = self.get_clock().now().to_msg()
+ goal_pose.state = FreeFlyerState(pose=FF_Pose2D(x=x, y=y, theta=th), twist=Twist2D())
+ return goal_pose
+
+ def state_cb(self, state) -> None:
+ """Read the state as it becomes available."""
+ self.state = state
+
+ def get_current_time(self) -> float:
+ secs, nsecs = self.get_clock().now().seconds_nanoseconds()
+ return secs + nsecs / 1e9
+
+ def publish_goal_cb(self) -> None:
+ """Publish the goal pose based on the interpolation of the plan."""
+ if self.plan is None:
+ self.get_logger().info("No plan available yet.")
+ return
+ current_time = self.get_current_time()
+ if current_time < self.plan["t"][0] or current_time > self.plan["t"][-1]:
+ self.get_logger().info("Current time outside of the plan time horizon.")
+ current_time = np.clip(current_time, self.plan["t"][0], self.plan["t"][-1])
+ x_goal = interp1d(self.plan["t"], self.plan["X"], axis=0)(current_time)
+ goal_pose = self.make_goal_pose(x_goal[0], x_goal[1])
+ self.ctrl_state_pub.publish(goal_pose)
+ self.get_logger().info("Published goal pose")
+
+ def recompute_plan_cb(self) -> None:
+ request = PathPlan.Request(horizon=20)
+
+ request.dynamics = "single_integrator_2D"
+ request.t0 = self.get_current_time()
+ x, y = self.state.state.pose.x, self.state.state.pose.y
+ request.x0 = to_ros_array(np.array([x, y]))
+
+ # we'll provide extra arguments to the cost function, which accepts params encoded as a JSON
+ request.cost = "my_cost"
+ X_ref = np.tile(np.array([2.5, 2]), (request.horizon, 1)).tolist()
+ R = np.tile(np.eye(2) * 1e2, (request.horizon, 1, 1)).tolist()
+ request.params_json = json.dumps(dict(X_ref=X_ref, R=R))
+
+ # send a request to the path planner
+ self.plan_futures.append(self.path_plan_client.call_async(request))
+
+
+####################################################################################################
+
+
+def main():
+ # spin the node up
+ rclpy.init()
+ navigation_node = NavigationNode()
+ rclpy.spin(navigation_node)
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ff_path_planning/ff_path_planning/examples/service_call.py b/ff_path_planning/ff_path_planning/examples/service_call.py
new file mode 100644
index 0000000..0fb5064
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/examples/service_call.py
@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import time
+import json
+
+import numpy as np
+
+from ff_srvs.srv import PathPlan
+import rclpy
+from rclpy.node import Node
+
+####################################################################################################
+
+
+def to_ros_array(x: np.ndarray):
+ return x.reshape(-1).tolist()
+
+
+def _send_request(node, cli):
+ request = PathPlan.Request()
+ request.dynamics = "single_integrator"
+ secs, nsecs = node.get_clock().now().seconds_nanoseconds()
+ request.t0 = secs + nsecs / 1e9
+ request.x0 = to_ros_array(np.array([5.0]))
+ # example json params, numpy arrays can be sent as (nested list)
+ # use: `arr.tolist()``
+ request.params_json = json.dumps(
+ dict(dt=0.1, data=(1, dict(a=1, b=2, x=np.array([1.0]).tolist())))
+ )
+ future = cli.call_async(request)
+ rclpy.spin_until_future_complete(node, future)
+ result = future.result()
+
+ N = request.horizon
+ ts = np.array(result.times)
+ X = np.array(result.states).reshape((N + 1, -1))
+ U = np.array(result.controls).reshape((N, -1))
+ L = np.array(result.feedback).reshape((N, X.shape[-1], U.shape[-1]))
+ return ts, X, U, L
+
+
+class ExamplePathPlanningClient(Node):
+ def __init__(self):
+ super().__init__("path_planning_client")
+ self.cli = self.create_client(PathPlan, "/path_planning")
+ while not self.cli.wait_for_service(timeout_sec=0.1):
+ self.get_logger().info("service not available, waiting again...")
+
+ def send_request(self):
+ print("Sending a request to the path planning service")
+ t = time.time()
+ ret = _send_request(self, self.cli)
+ self.get_logger().info(f"Calling the path planning took {time.time() - t:.4e} seconds")
+ return ret
+
+
+####################################################################################################
+
+
+def main():
+ rclpy.init()
+
+ example_client = ExamplePathPlanningClient()
+ ts, X, U, L = example_client.send_request()
+ example_client.get_logger().info(f"ts = {ts}")
+ example_client.get_logger().info(f"X = {X}")
+ example_client.get_logger().info(f"U = {U}")
+ example_client.get_logger().info(f"L = {L}")
+
+ example_client.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ff_path_planning/ff_path_planning/examples/setting_goal.py b/ff_path_planning/ff_path_planning/examples/setting_goal.py
new file mode 100644
index 0000000..2830405
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/examples/setting_goal.py
@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+"""Broadcast a simple static goal to the controller."""
+
+import rclpy
+from rclpy.node import Node
+from ff_msgs.msg import FreeFlyerStateStamped, FreeFlyerState, Twist2D, Pose2D as FF_Pose2D
+
+####################################################################################################
+
+
+class SimpleGoalNode(Node):
+ def __init__(self):
+ """A path planning service node based on the `pmpc` library."""
+ super().__init__("simple_goal")
+ self.ctrl_state_pub = self.create_publisher(FreeFlyerStateStamped, "ctrl/state", 10)
+ self.state_sub = self.create_subscription(
+ FreeFlyerStateStamped, "gt/state", self.state_cb, 10
+ )
+ self.state, self.goal_pose = None, None
+
+ self.already_published = False
+ self.create_timer(1.0, self.publish_goal)
+
+ pub = self.create_publisher(FreeFlyerStateStamped, "state_init", 10)
+ msg = FreeFlyerStateStamped()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.state = FreeFlyerState(twist=Twist2D(), pose=FF_Pose2D())
+ pub.publish(msg)
+
+ def state_cb(self, state):
+ if self.goal_pose is None:
+ x, y, th = state.state.pose.x, state.state.pose.y, state.state.pose.theta
+ self.goal_pose = FreeFlyerStateStamped()
+ self.goal_pose.header.stamp = self.get_clock().now().to_msg()
+ self.goal_pose.state = FreeFlyerState(
+ pose=FF_Pose2D(x=x + 1, y=y + 2, theta=th), twist=Twist2D()
+ )
+ self.state = state
+
+ def publish_goal(self):
+ # if self.already_published:
+ # return
+ if self.goal_pose is None:
+ return
+ self.goal_pose.header.stamp = self.get_clock().now().to_msg()
+ self.ctrl_state_pub.publish(self.goal_pose)
+ self.get_logger().info("Published goal pose")
+ print(f"Published goal pose: {self.goal_pose.state.pose.x}, {self.goal_pose.state.pose.y}")
+ print(f"Actual pose {self.state.state.pose.x}, {self.state.state.pose.y}")
+ self.already_published = True
+
+
+####################################################################################################
+
+
+def main():
+ # spin the node up
+ rclpy.init()
+ path_planning_service = SimpleGoalNode()
+ rclpy.spin(path_planning_service)
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ff_path_planning/ff_path_planning/import_costs_and_dynamics.py b/ff_path_planning/ff_path_planning/import_costs_and_dynamics.py
new file mode 100644
index 0000000..39b4fa1
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/import_costs_and_dynamics.py
@@ -0,0 +1,127 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import sys
+import os
+from typing import Optional, Union
+from pathlib import Path
+from importlib import import_module, reload
+
+DYNAMICS_MODULES, COSTS_MODULES = {}, {}
+
+####################################################################################################
+
+
+def find_all_dynamics_and_costs(root_path: Optional[Union[Path, str]] = None):
+ """Locate all python files in the `dynamics` and `costs` folder given a package root path."""
+ root_path = Path(root_path) if root_path is not None else Path(__file__).absolute().parent
+ all_dynamics = [
+ x
+ for x in os.listdir(root_path / "dynamics")
+ if Path(x).suffix == ".py" and x != "__init__.py"
+ ]
+ all_costs = [
+ x for x in os.listdir(root_path / "costs") if Path(x).suffix == ".py" and x != "__init__.py"
+ ]
+ return all_dynamics, all_costs
+
+
+def find_pkg_source_dir(root_path: Optional[Union[Path, str]] = None):
+ """Find where the source of the ff_path_planning package is located. This
+ allows hot-reloading dynamics and cost definitions during development."""
+ root_path = Path(root_path) if root_path is not None else Path(__file__).absolute().parent
+ ros_ws_path = root_path.parents[5]
+ msg = f"We're attempting to guess the ROS workspace path is {ros_ws_path}. Is this correct?"
+
+ assert (ros_ws_path / "install").exists() and (ros_ws_path / "src").exists(), msg
+ this_pkg_name = root_path.parts[-1]
+ pkg_paths = sum(
+ [
+ [ros_ws_path / "src" / root / dir for dir in dirs if dir == this_pkg_name]
+ for (root, dirs, fnames) in os.walk(ros_ws_path / "src")
+ ],
+ [],
+ )
+ pkg_path = sorted(pkg_paths, key=lambda x: len(x.parts))[0] # pick the shortest path
+ return pkg_path
+
+
+def append_to_path(root_path: Optional[Union[Path, str]] = None):
+ """Append the installed location of the ff_path_planning package and the
+ local development (editable package)."""
+ root_path = Path(root_path) if root_path is not None else Path(__file__).absolute().parent
+
+ # append ROS install path first to the end ########################
+ dynamics_path = Path(root_path) / "dynamics"
+ costs_path = Path(root_path) / "costs"
+ for path in [dynamics_path, costs_path]:
+ if str(path) not in sys.path:
+ sys.path.append(str(path))
+
+ # insert the local development path first #########################
+ # pkg_path = find_pkg_source_dir(root_path)
+ # this_pkg_name = root_path.parts[-1]
+ # msg = f"We're attempting to guess the package path is {pkg_path}. Is this correct?"
+ # dynamics_path = pkg_path / this_pkg_name / "dynamics"
+ # costs_path = pkg_path / this_pkg_name / "costs"
+ # assert dynamics_path.exists() and costs_path.exists(), msg
+ # for path in [dynamics_path, costs_path]:
+ # if str(path) not in sys.path:
+ # sys.path.insert(0, str(path))
+
+
+def load_all_modules(root_path: Optional[Union[Path, str]] = None):
+ all_dynamics, all_costs = find_all_dynamics_and_costs(root_path)
+
+ # pkg_source = find_pkg_source_dir(root_path)
+ # all_dynamics_dev, all_costs_dev = find_all_dynamics_and_costs(pkg_source / pkg_source.parts[-1])
+ # all_dynamics += all_dynamics_dev
+ # all_costs += all_costs_dev
+
+ for dynamics in all_dynamics:
+ mod_name = str(Path(dynamics).stem)
+ try:
+ if mod_name in DYNAMICS_MODULES:
+ reload(DYNAMICS_MODULES[mod_name])
+ else:
+ DYNAMICS_MODULES[mod_name] = import_module(mod_name)
+ except:
+ pass
+
+ for cost in all_costs:
+ mod_name = str(Path(cost).stem)
+ try:
+ if mod_name in COSTS_MODULES:
+ reload(COSTS_MODULES[mod_name])
+ else:
+ COSTS_MODULES[mod_name] = import_module(mod_name)
+ except:
+ pass
+
+
+####################################################################################################
+
+# this should happen every time the module is imported, **not** just when it's called as a script
+append_to_path()
+load_all_modules()
+
+####################################################################################################
diff --git a/ff_path_planning/ff_path_planning/path_planning.py b/ff_path_planning/ff_path_planning/path_planning.py
new file mode 100644
index 0000000..6a0ff15
--- /dev/null
+++ b/ff_path_planning/ff_path_planning/path_planning.py
@@ -0,0 +1,227 @@
+#!/usr/bin/env python3
+
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+"""Path plan and broadcast an interpolated plan as a goal to the controller."""
+
+
+import time
+import traceback
+import json
+import math
+from warnings import warn
+
+import rclpy
+from rclpy.node import Node
+from ff_srvs.srv import PathPlan
+from multiprocessing import Process, set_start_method
+
+import numpy as np
+
+try:
+ from pmpc import solve, Problem
+except ImportError:
+ warn(
+ "#" * 80
+ + "\n"
+ + "#" * 80
+ + "\n"
+ + "It looks like you don't have the `pmpc` library installed. "
+ + "Please install it from https://github.com/StanfordASL/pmpc\n"
+ + "#" * 80
+ + "\n"
+ + "#" * 80
+ )
+
+
+from .import_costs_and_dynamics import DYNAMICS_MODULES, COSTS_MODULES, load_all_modules
+
+####################################################################################################
+
+
+def to_ros_array(x):
+ return x.reshape(-1).tolist()
+
+
+def example_request_for_jit():
+ """Send an example request to the path planning service to trigger JIT compilation."""
+ rclpy.init()
+ node = Node("path_planning_client")
+ node.get_logger().info("Starting an example request node")
+ cli = node.create_client(PathPlan, "path_planning")
+ while not cli.wait_for_service(timeout_sec=0.1):
+ pass
+ node.get_logger().info("Path Planning Service is up")
+ request = PathPlan.Request()
+ request.dynamics = "single_integrator"
+ secs, nsecs = rclpy.clock.Clock().now().seconds_nanoseconds()
+ request.t0 = secs + nsecs / 1e9
+ request.x0 = to_ros_array(np.array([5.0]))
+
+ t = time.time()
+ node.get_logger().info("Sending compilation request")
+ future = cli.call_async(request)
+ rclpy.spin_until_future_complete(node, future)
+ result = future.result()
+ node.get_logger().info(f"Precompilation took {time.time() - t:.4} seconds")
+
+ N = request.horizon
+ ts = np.array(result.times)
+ X = np.array(result.states).reshape((N + 1, -1))
+ U = np.array(result.controls).reshape((N, -1))
+ L = np.array(result.feedback).reshape((N, X.shape[-1], U.shape[-1]))
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+####################################################################################################
+
+
+class PathPlanningService(Node):
+ def __init__(self):
+ """A path planning service node based on the `pmpc` library."""
+ super().__init__("path_planning")
+ self.get_logger().info("Starting the path planning service")
+ self.srv = self.create_service(PathPlan, "path_planning", self.compute_plan)
+ self.get_logger().info("Available dynamics: [" + ", ".join(DYNAMICS_MODULES.keys()) + "]")
+ self.get_logger().info("Available costs: [" + ", ".join(COSTS_MODULES.keys()) + "]")
+
+ def _empty_plan(self, request, response):
+ """Construct and empty plan, filled with NaNs to indicate failure."""
+ xdim, udim, N = request.xdim, request.udim, request.horizon
+ xdim = xdim if xdim > 0 else 1
+ udim = udim if udim > 0 else 1
+ response.times = to_ros_array(math.nan * np.ones(N + 1))
+ response.states = to_ros_array(math.nan * np.ones((N + 1, xdim)))
+ response.controls = to_ros_array(math.nan * np.ones((N, udim)))
+ response.feedback = to_ros_array(math.nan * np.ones((N, xdim, udim)))
+ return response
+
+ def _resolve_dimensions(self, request):
+ """Resolve the dimensions, [xdim, udim] based on provided values or the dynamics module."""
+ dyn_mod = DYNAMICS_MODULES[request.dynamics]
+ xdim, udim = request.xdim, request.udim
+ if hasattr(dyn_mod, "XDIM") or hasattr(dyn_mod, "xdim"):
+ mod_xdim = getattr(dyn_mod, "XDIM", getattr(dyn_mod, "xdim", None))
+ if request.xdim > 0:
+ assert request.xdim == mod_xdim
+ xdim = mod_xdim
+ if hasattr(dyn_mod, "UDIM") or hasattr(dyn_mod, "udim"):
+ mod_udim = getattr(dyn_mod, "UDIM", getattr(dyn_mod, "udim", None))
+ if request.udim > 0:
+ assert request.udim == mod_udim
+ udim = mod_udim
+ assert xdim > 0 and udim > 0
+ return dict(xdim=xdim, udim=udim, N=request.horizon)
+
+ def compute_plan(self, request, response):
+ """Main service callback for computing the optimal plan."""
+ load_all_modules() # reload modules that the user might have added
+
+ # validate the request ###################
+ if request.dynamics not in DYNAMICS_MODULES or request.cost not in COSTS_MODULES:
+ msg = f"Dynamics `{request.dynamics}` or cost `{request.cost}` not found"
+ msg += "\nDynamics available: " + ", ".join(DYNAMICS_MODULES.keys())
+ msg += "\nCosts available: " + ", ".join(COSTS_MODULES.keys())
+ self.get_logger().error(msg)
+ self.get_logger().info("Path planning node is still healthy.")
+ return self._empty_plan(request, response)
+ try:
+ dims = self._resolve_dimensions(request)
+ except AssertionError:
+ self.get_logger().error("Invalid dimensions: xdim, udim")
+ self.get_logger().info("Path planning node is still healthy.")
+ return self._empty_plan(request, response)
+ # validate the request ###################
+
+ # parse extra problem parameters #########
+ params = None
+ try:
+ if len(request.params_json) > 0:
+ params = json.loads(request.params_json)
+ except ValueError:
+ self.get_logger().error("Error deserializing params_json")
+ self.get_logger().error(traceback.format_exc())
+ self.get_logger().info("Path planning node is still healthy.")
+ return self._empty_plan(request, response)
+ # parse extra problem parameters #########
+
+ # construct the problem ##################
+ try:
+ if params is None:
+ cost = COSTS_MODULES[request.cost].cost(**dims)
+ f_fx_fu_fn = DYNAMICS_MODULES[request.dynamics].f_fx_fu_fn
+ else:
+ cost = COSTS_MODULES[request.cost].cost(**dims, params=params)
+ f_fx_fu_fn = lambda X, U: DYNAMICS_MODULES[request.dynamics].f_fx_fu_fn(
+ X, U, params=params
+ )
+ x0 = np.array(request.x0)
+ p = Problem(x0=x0, f_fx_fu_fn=f_fx_fu_fn, **dims, **cost)
+ p.max_it = request.max_it
+ p.verbose = True
+ except:
+ self.get_logger().error(traceback.format_exc())
+ self.get_logger().info("Path planning node is still healthy.")
+ return self._empty_plan(request, response)
+ # construct the problem ##################
+
+ # solve ##################################
+ try:
+ X, U, _ = solve(**p)
+ except:
+ self.get_logger().error(traceback.format_exc())
+ self.get_logger().info("Path planning node is still healthy.")
+ return self._empty_plan(request, response)
+ # solve ##################################
+
+ # fill the response ######################
+ response.times = to_ros_array(request.t0 + np.arange(request.horizon + 1) * request.dt)
+ response.states = to_ros_array(X)
+ response.controls = to_ros_array(U)
+ response.feedback = to_ros_array(
+ math.nan * np.ones((dims["N"], dims["xdim"], dims["udim"]))
+ )
+ # fill the response ######################
+ self.get_logger().info("Path planning service finished")
+ return response
+
+
+####################################################################################################
+
+
+def main():
+ # send an example request to trigger the JIT compilation immediately
+ set_start_method("spawn")
+ p = Process(target=example_request_for_jit)
+ p.start()
+
+ # spin the node up
+ rclpy.init()
+ path_planning_service = PathPlanningService()
+ rclpy.spin(path_planning_service)
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ff_path_planning/package.xml b/ff_path_planning/package.xml
new file mode 100644
index 0000000..4992f98
--- /dev/null
+++ b/ff_path_planning/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ ff_path_planning
+ 0.0.0
+ MPC Path Planning Service
+ rdyro
+ MIT License
+
+ std_msgs
+ std_srvs
+ ff_srvs
+ python3-scipy
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/ff_path_planning/resource/ff_path_planning b/ff_path_planning/resource/ff_path_planning
new file mode 100644
index 0000000..e69de29
diff --git a/ff_path_planning/setup.cfg b/ff_path_planning/setup.cfg
new file mode 100644
index 0000000..e030362
--- /dev/null
+++ b/ff_path_planning/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/ff_path_planning
+[install]
+install_scripts=$base/lib/ff_path_planning
diff --git a/ff_path_planning/setup.py b/ff_path_planning/setup.py
new file mode 100644
index 0000000..5f8d476
--- /dev/null
+++ b/ff_path_planning/setup.py
@@ -0,0 +1,28 @@
+from setuptools import setup, find_packages
+
+package_name = "ff_path_planning"
+
+setup(
+ name=package_name,
+ version="0.1.0",
+ packages=find_packages(),
+ data_files=[
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ ],
+ install_requires=["setuptools"],
+ zip_safe=True,
+ maintainer="rdyro",
+ maintainer_email="rdyro@stanford.edu",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
+ entry_points={
+ "console_scripts": [
+ "path_planning = ff_path_planning.path_planning:main",
+ "examples:service_call = ff_path_planning.examples.service_call:main",
+ "examples:setting_goal = ff_path_planning.examples.setting_goal:main",
+ "examples:path_planning = ff_path_planning.examples.path_planning:main",
+ ],
+ },
+)
diff --git a/ff_path_planning/test/test_copyright.py b/ff_path_planning/test/test_copyright.py
new file mode 100644
index 0000000..defd60e
--- /dev/null
+++ b/ff_path_planning/test/test_copyright.py
@@ -0,0 +1,24 @@
+# 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.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/ff_srvs/CMakeLists.txt b/ff_srvs/CMakeLists.txt
new file mode 100644
index 0000000..64671cf
--- /dev/null
+++ b/ff_srvs/CMakeLists.txt
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.8)
+project(ff_srvs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "srv/PathPlan.srv"
+ DEPENDENCIES std_msgs std_srvs
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/ff_srvs/package.xml b/ff_srvs/package.xml
new file mode 100644
index 0000000..bc4f08e
--- /dev/null
+++ b/ff_srvs/package.xml
@@ -0,0 +1,29 @@
+
+
+
+ ff_srvs
+ 0.0.0
+ TODO: Package description
+ alvin
+ TODO: License declaration
+
+ ament_cmake
+
+ std_msgs
+ std_srvs
+
+ rosidl_default_generators
+
+ rosidl_default_runtime
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ rosidl_interface_packages
+
+
+
+ ament_cmake
+
+
\ No newline at end of file
diff --git a/ff_srvs/srv/PathPlan.srv b/ff_srvs/srv/PathPlan.srv
new file mode 100644
index 0000000..c611d56
--- /dev/null
+++ b/ff_srvs/srv/PathPlan.srv
@@ -0,0 +1,15 @@
+string dynamics
+string cost "default_cost"
+float64[] x0
+float64 t0
+float64 dt 0.25
+int64 horizon 20
+int64 xdim -1
+int64 udim -1
+int64 max_it 20
+string params_json
+---
+float64[] times
+float64[] states
+float64[] controls
+float64[] feedback
\ No newline at end of file
diff --git a/ff_viz/rviz/default.rviz b/ff_viz/rviz/default.rviz
index 4f90037..6b4417d 100644
--- a/ff_viz/rviz/default.rviz
+++ b/ff_viz/rviz/default.rviz
@@ -11,7 +11,7 @@ Panels:
- /TF1
- /TF1/Frames1
Splitter Ratio: 0.5
- Tree Height: 637
+ Tree Height: 991
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -115,6 +115,34 @@ Visualization Manager:
{}
Update Interval: 0
Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /robot/path_marker
+ Value: true
Enabled: true
Global Options:
Background Color: 246; 245; 244
@@ -161,7 +189,7 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
- Distance: 2.9315927028656006
+ Distance: 10.080626487731934
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -176,10 +204,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.48979541659355164
+ Pitch: 0.8647950887680054
Target Frame:
Value: Orbit (rviz)
- Yaw: 4.038604736328125
+ Yaw: 3.033583641052246
Saved: ~
Window Geometry:
Displays:
@@ -187,7 +215,7 @@ Window Geometry:
Height: 1022
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd0000000400000000000001f700000344fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000344000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000344fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000344000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007480000005afc0100000002fb0000000800540069006d0065010000000000000748000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e60000034400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000400000000000001f7000004a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000004a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000005afc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000654000004a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -196,6 +224,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 1864
- X: 105
- Y: 270
+ Width: 2486
+ X: 74
+ Y: 27
\ No newline at end of file
diff --git a/freeflyer/launch/sim_path_planning.launch.py b/freeflyer/launch/sim_path_planning.launch.py
new file mode 100644
index 0000000..5d007a3
--- /dev/null
+++ b/freeflyer/launch/sim_path_planning.launch.py
@@ -0,0 +1,77 @@
+# MIT License
+#
+# Copyright (c) 2023 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+ robot_name = LaunchConfiguration("robot_name")
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument("robot_name", default_value="robot"),
+ IncludeLaunchDescription(
+ PathJoinSubstitution(
+ [
+ FindPackageShare("ff_sim"),
+ "launch",
+ "single.launch.py",
+ ]
+ ),
+ launch_arguments={"robot_name": robot_name}.items(),
+ ),
+ IncludeLaunchDescription(
+ PathJoinSubstitution(
+ [
+ FindPackageShare("ff_viz"),
+ "launch",
+ "ff_viz.launch.py",
+ ]
+ ),
+ launch_arguments={"robot_name": robot_name}.items(),
+ ),
+ Node(
+ package="ff_control",
+ executable="pd_ctrl_node",
+ name="pd_ctrl_node",
+ namespace=robot_name,
+ ),
+ # path planning ############################################################################
+ Node(
+ package="ff_path_planning",
+ executable="examples:path_planning",
+ name="simple_goal_broadcaster",
+ namespace=robot_name,
+ ),
+ # we want to launch this node separately, for better output
+ # Node(
+ # package="ff_path_planning",
+ # executable="path_planning",
+ # name="path_planner_node",
+ # namespace=robot_name,
+ # ),
+ ]
+ )