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, + # ), + ] + )