From 094d6de8dabce16881da4064dac5ab676276c8a5 Mon Sep 17 00:00:00 2001 From: Christopher Agia Date: Fri, 21 Apr 2023 17:22:38 -0700 Subject: [PATCH 01/56] Add preliminary python devel for ff_control Signed-off-by: Christopher Agia --- ff_control/CMakeLists.txt | 8 ++++++ ff_control/ff_control/__init__.py | 0 ff_control/ff_control/linear_ctrl.py | 42 ++++++++++++++++++++++++++++ ff_control/ff_control/ll_ctrl.py | 34 ++++++++++++++++++++++ ff_control/ff_control/wrench_ctrl.py | 0 ff_control/package.xml | 2 ++ ff_control/scripts/ll_ctrl_test.py | 28 +++++++++++++++++++ 7 files changed, 114 insertions(+) create mode 100644 ff_control/ff_control/__init__.py create mode 100644 ff_control/ff_control/linear_ctrl.py create mode 100644 ff_control/ff_control/ll_ctrl.py create mode 100644 ff_control/ff_control/wrench_ctrl.py create mode 100755 ff_control/scripts/ll_ctrl_test.py diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 50f6b46..877bd45 100644 --- a/ff_control/CMakeLists.txt +++ b/ff_control/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(geometry_msgs REQUIRED) @@ -39,6 +40,11 @@ install( INCLUDES DESTINATION include ) +install(PROGRAMS + scripts/ll_ctrl_test.py + DESTINATION lib/${PROJECT_NAME} +) + add_executable(wrench_ctrl_node src/nodes/wrench_ctrl_node.cpp) target_link_libraries(wrench_ctrl_node ctrl_lib) @@ -52,6 +58,8 @@ target_link_libraries(key_teleop_node ctrl_lib) install(TARGETS wrench_ctrl_node pd_ctrl_node key_teleop_node DESTINATION lib/${PROJECT_NAME}) +ament_python_install_package(${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/ff_control/ff_control/__init__.py b/ff_control/ff_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ff_control/ff_control/linear_ctrl.py b/ff_control/ff_control/linear_ctrl.py new file mode 100644 index 0000000..af0c1f5 --- /dev/null +++ b/ff_control/ff_control/linear_ctrl.py @@ -0,0 +1,42 @@ +from typing import Sequence + +import rclpy +from rclpy.node import Node + +import numpy as np + +from ff_msgs.msg import Wrench2D +from ff_msgs.msg import FreeFlyerStateStamped +from ff_params import RobotParams + + +class LinearController(Node): + + STATE_DIM = 6 + CONTROL_DIM = 3 + + def __init__(self, node_name="linear_ctrl_node"): + super().__init__(node_name) + self.p = RobotParams(self) + self._wrench_body_pub = self.create_publisher(Wrench2D, "ctrl/wrench_body", 10) + self._wrench_world_pub = self.create_publisher(Wrench2D, "ctrl/wrench_world", 10) + self._pose_sub = self.create_subscription(FreeFlyerStateStamped, "gt/pose", self.pose_cb, 10) + self._state_ready = False + self._state_vector = np.zeros(self.STATE_DIM) + + def state_callback(self, msg: FreeFlyerStateStamped) -> None: + + pass + + + def state_is_ready(): + pass + + def get_state(): + pass + + def send_control(): + pass + + def state_ready_callback(): + pass diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py new file mode 100644 index 0000000..d50cf9a --- /dev/null +++ b/ff_control/ff_control/ll_ctrl.py @@ -0,0 +1,34 @@ +from typing import Sequence + +import rclpy +from rclpy.node import Node + +import numpy as np + +from ff_msgs.msg import ThrusterCommand +from ff_msgs.msg import WheelVelCommand +from ff_params import RobotParams + + +class LowLevelController(Node): + + def __init__(self): + super().__init__("ll_ctrl_node") + self.p = RobotParams(self) + self._thruster_pub = self.create_publisher(ThrusterCommand, "commands/duty_cycle", 10) + self._wheel_pub = self.create_publisher(WheelVelCommand, "commands/velocity", 10) + + def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: + if len(duty_cycle) != len(ThrusterCommand().duty_cycle): + self.get_logger().error("Incompatible thruster length sent.") + return + msg = ThrusterCommand() + msg.header.stamp = self.get_clock().now().to_msg() + msg.duty_cycle = duty_cycle + self._thruster_pub.publish(msg) + + def set_wheel_velocity(self, velocity: float) -> None: + msg = WheelVelCommand() + msg.header.stamp = self.get_clock().now().to_msg() + msg.velocity = velocity + self._wheel_pub.publish(msg) diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py new file mode 100644 index 0000000..e69de29 diff --git a/ff_control/package.xml b/ff_control/package.xml index e69c42f..555a5e9 100644 --- a/ff_control/package.xml +++ b/ff_control/package.xml @@ -8,11 +8,13 @@ TODO: License declaration ament_cmake + ament_cmake_python eigen3_cmake_module eigen rclcpp + rclpy geometry_msgs ff_msgs diff --git a/ff_control/scripts/ll_ctrl_test.py b/ff_control/scripts/ll_ctrl_test.py new file mode 100755 index 0000000..13e8c83 --- /dev/null +++ b/ff_control/scripts/ll_ctrl_test.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node + +import numpy as np + +from ff_control.ll_ctrl import LowLevelController + + +class TestLowLevelController(LowLevelController): + + def __init__(self): + super().__init__() + self.timer = self.create_timer(1.0, self.set_thrust_command) + + def set_thrust_command(self) -> None: + self.set_thrust_duty_cycle(np.ones(8) * 0.5) + + +def main(args=None): + rclpy.init(args=args) + debug_ctrl = TestLowLevelController() + rclpy.spin(debug_ctrl) + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file From f4371f6e8ebf5c97c386a3d5c26228f3daec0c6c Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 25 Apr 2023 11:44:36 -0700 Subject: [PATCH 02/56] cpp node still works --- ff_control/CMakeLists.txt | 18 +-- ff_control/ff_control/linear_ctrl.py | 147 +++++++++++++++--- ff_control/ff_control/ll_ctrl.py | 29 +++- ff_control/ff_control/wrench_ctrl.py | 85 ++++++++++ ff_control/include/ff_control/linear_ctrl.hpp | 8 +- ff_control/include/ff_control/ll_ctrl.hpp | 2 +- ff_control/scripts/ll_ctrl_test.py | 28 ---- ff_control/scripts/pd_ctrl_py_node | 69 ++++++++ ff_control/src/nodes/pd_ctrl_node.cpp | 2 + freeflyer/launch/sim_key_teleop.launch.py | 2 +- freeflyer/launch/sim_pd.launch.py | 3 +- 11 files changed, 322 insertions(+), 71 deletions(-) delete mode 100755 ff_control/scripts/ll_ctrl_test.py create mode 100755 ff_control/scripts/pd_ctrl_py_node diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 877bd45..2ac573b 100644 --- a/ff_control/CMakeLists.txt +++ b/ff_control/CMakeLists.txt @@ -41,21 +41,21 @@ install( ) install(PROGRAMS - scripts/ll_ctrl_test.py + scripts/pd_ctrl_py_node DESTINATION lib/${PROJECT_NAME} ) -add_executable(wrench_ctrl_node src/nodes/wrench_ctrl_node.cpp) -target_link_libraries(wrench_ctrl_node ctrl_lib) +add_executable(wrench_ctrl_cpp_node src/nodes/wrench_ctrl_node.cpp) +target_link_libraries(wrench_ctrl_cpp_node ctrl_lib) -add_executable(pd_ctrl_node src/nodes/pd_ctrl_node.cpp) -target_link_libraries(pd_ctrl_node ctrl_lib) -ament_target_dependencies(pd_ctrl_node geometry_msgs) +add_executable(pd_ctrl_cpp_node src/nodes/pd_ctrl_node.cpp) +target_link_libraries(pd_ctrl_cpp_node ctrl_lib) +ament_target_dependencies(pd_ctrl_cpp_node geometry_msgs) -add_executable(key_teleop_node src/nodes/key_teleop_node.cpp) -target_link_libraries(key_teleop_node ctrl_lib) +add_executable(key_teleop_cpp_node src/nodes/key_teleop_node.cpp) +target_link_libraries(key_teleop_cpp_node ctrl_lib) -install(TARGETS wrench_ctrl_node pd_ctrl_node key_teleop_node +install(TARGETS wrench_ctrl_cpp_node pd_ctrl_cpp_node key_teleop_cpp_node DESTINATION lib/${PROJECT_NAME}) ament_python_install_package(${PROJECT_NAME}) diff --git a/ff_control/ff_control/linear_ctrl.py b/ff_control/ff_control/linear_ctrl.py index af0c1f5..49c2872 100644 --- a/ff_control/ff_control/linear_ctrl.py +++ b/ff_control/ff_control/linear_ctrl.py @@ -1,42 +1,147 @@ -from typing import Sequence - import rclpy -from rclpy.node import Node +import copy import numpy as np +import typing as T +from ff_control.wrench_ctrl import WrenchController from ff_msgs.msg import Wrench2D +from ff_msgs.msg import FreeFlyerState from ff_msgs.msg import FreeFlyerStateStamped from ff_params import RobotParams -class LinearController(Node): +class LinearController(WrenchController): + """ + base class for any linear controller + + state definition: [x, y, theta, vx, vy, wz] + control definition: [fx, fy, tz] + + Note: the current implementation is not thread safe, if you want to use a + multi-threaded exector, use the CPP version in linear_ctrl.hpp + """ STATE_DIM = 6 CONTROL_DIM = 3 def __init__(self, node_name="linear_ctrl_node"): super().__init__(node_name) - self.p = RobotParams(self) - self._wrench_body_pub = self.create_publisher(Wrench2D, "ctrl/wrench_body", 10) - self._wrench_world_pub = self.create_publisher(Wrench2D, "ctrl/wrench_world", 10) - self._pose_sub = self.create_subscription(FreeFlyerStateStamped, "gt/pose", self.pose_cb, 10) + self._state_sub = self.create_subscription(FreeFlyerStateStamped, + "gt/state", self._state_callback, 10) self._state_ready = False - self._state_vector = np.zeros(self.STATE_DIM) + self._state_stamped = FreeFlyerStateStamped() - def state_callback(self, msg: FreeFlyerStateStamped) -> None: - - pass - - - def state_is_ready(): - pass + @property + def feedback_gain_shape(self) -> T.Tuple[int, int]: + """get shape of the feedback control gain matrix - def get_state(): - pass + Returns: + T.Tuple[int, int]: (CONTROL_DIM, STATE_DIM) + """ + return (self.CONTROL_DIM, self.STATE_DIM) - def send_control(): - pass + def get_state(self) -> T.Optional[FreeFlyerState]: + """get the current latest state + + Returns: + T.Optional[FreeFlyerState]: the current state, None if not available + """ + if not self._state_ready: + self.get_logger().error("get_state failed: state not yet ready") + return None + + return self._state_stamped.state + + def send_control(self, state_des: T.Union[FreeFlyerState, np.ndarray], K: np.ndarray) -> None: + """send desirable target state for linear control + + Args: + state_des (T.Union[FreeFlyerState, np.ndarray]): desired state + K (np.ndarray): feedback control matrix (i.e. u = Kx) + """ + if not self._state_ready: + self.get_logger().warn("send_control ignored, state not yet ready") + return + + if K.shape != self.feedback_gain_shape: + self.get_logger().error("send_control failed: incompatible gain matrix shape") + return + + # convert desired state to vector form + if isinstance(state_des, FreeFlyerState): + state_des = self.state2vec(state_des) + + state_vector = self.state2vec(self.get_state()) + state_delta = state_des - state_vector + # wrap angle delta to [-pi, pi] + state_delta[2] = (state_delta[2] + np.pi) % (2 * np.pi) - np.pi - def state_ready_callback(): + u = K @ state_delta + + wrench_world = Wrench2D() + wrench_world.fx = u[0] + wrench_world.fy = u[1] + wrench_world.tz = u[2] + self.set_world_wrench(wrench_world, state_vector[2]) + + def state_ready_callback(self) -> None: + """callback invoked when the first state measurement comes in + Sub-classes should override this function + """ pass + + @staticmethod + def state2vec(state: FreeFlyerState) -> np.ndarray: + """convert state message to state vector + + Args: + state (FreeFlyerState): state message + + Returns: + np.ndarray: state vector + """ + return np.array([ + state.pose.x, + state.pose.y, + state.pose.theta, + state.twist.vx, + state.twist.vy, + state.twist.wz, + ]) + + @staticmethod + def vec2state(vec: np.ndarray) -> FreeFlyerState: + """convert state vector to state message + + Args: + vec (np.ndarray): state vector + + Returns: + FreeFlyerState: state message + """ + state = FreeFlyerState() + state.pose.x = vec[0] + state.pose.y = vec[1] + state.pose.theta = vec[2] + state.twist.vx = vec[3] + state.twist.vy = vec[4] + state.twist.wz = vec[5] + + return state + + def state_is_ready(self) -> bool: + """check if state is ready + + Returns: + bool: True if state is ready, False otherwise + """ + return self._state_ready + + def _state_callback(self, msg: FreeFlyerStateStamped) -> None: + self._state_stamped = copy.deepcopy(msg.state) + + if not self._state_ready: + self._state_ready = True + self.state_ready_callback() + diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index d50cf9a..1df8317 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -1,5 +1,3 @@ -from typing import Sequence - import rclpy from rclpy.node import Node @@ -12,13 +10,22 @@ class LowLevelController(Node): - def __init__(self): - super().__init__("ll_ctrl_node") + def __init__(self, node_name: str = "ll_ctrl_node") -> None: + super().__init__(node_name) + + # robot parameters that can be accessed by sub-classes self.p = RobotParams(self) - self._thruster_pub = self.create_publisher(ThrusterCommand, "commands/duty_cycle", 10) - self._wheel_pub = self.create_publisher(WheelVelCommand, "commands/velocity", 10) - + + # low level control publishers + self._thruster_pub = self.create_publisher(ThrusterCommand, "ctrl/duty_cycle", 10) + self._wheel_pub = self.create_publisher(WheelVelCommand, "ctrl/velocity", 10) + def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: + """send command to set the thrusters duty cycles + + Args: + duty_cycle (np.ndarray): duty cycle for each thrust (in [0, 1]) + """ if len(duty_cycle) != len(ThrusterCommand().duty_cycle): self.get_logger().error("Incompatible thruster length sent.") return @@ -28,7 +35,15 @@ def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: self._thruster_pub.publish(msg) def set_wheel_velocity(self, velocity: float) -> None: + """send command to set the inertial wheel velocity + + TODO(alvin): suppor this or remove? + + Args: + velocity (float): angular velocity in [rad/s] + """ msg = WheelVelCommand() msg.header.stamp = self.get_clock().now().to_msg() msg.velocity = velocity self._wheel_pub.publish(msg) + diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index e69de29..a1f3e66 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -0,0 +1,85 @@ +import numpy as np + +import rclpy + +from ff_control.ll_ctrl import LowLevelController +from ff_msgs.msg import Wrench2D + + +class WrenchController(LowLevelController): + + def __init__(self, node_name: str = "wrench_ctrl_node") -> None: + super().__init__(node_name) + + def set_body_wrench(self, wrench_body: Wrench2D, use_wheel: bool = False) -> None: + """set wrench in body frame + + Args: + wrench_body (Wrench2D): wrench in body frame + use_wheel (bool): set to ture to use the inertial wheel + (TODO(alvin): unsupported) + """ + if use_wheel: + self.get_logger().error("set_wrench failed: use_wheel not implemented") + return + + duty_cycle = np.zeros(8) + wrench_body_clipped = self.clip_wrench(wrench_body) + + # convert force + u_Fx = wrench_body_clipped.fx / (2 * self.p.actuators["F_max_per_thruster"]) + u_Fy = wrench_body_clipped.fy / (2 * self.p.actuators["F_max_per_thruster"]) + if u_Fx > 0: + duty_cycle[2] = u_Fx + duty_cycle[5] = u_Fx; + else: + duty_cycle[1] = -u_Fx + duty_cycle[6] = -u_Fx + if u_Fy > 0: + duty_cycle[4] = u_Fy + duty_cycle[7] = u_Fy + else: + duty_cycle[0] = -u_Fy + duty_cycle[3] = -u_Fy + + # convert torque + u_M = wrench_body_clipped.tz / (4 * self.p.actuators["F_max_per_thruster"] \ + * self.p.actuators["thursters_lever_arm"]) + if u_M > 0: + duty_cycle[[1, 3, 5, 7]] += u_M + else: + duty_cycle[[0, 2, 4, 6]] += -u_M + + # clip to [0, 1] + duty_cycle = np.clip(duty_cycle, 0., 1.) + + self.set_thrust_duty_cycle(duty_cycle) + + def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: + """set wrench in world frame + + Args: + wrench_world (Wrench2D): wrench in world frame + theta (float): rotational state of the freeflyer + """ + cos_theta = np.cos(theta) + sin_theta = np.sin(theta) + + wrench_body = Wrench2D() + wrench_body.fx = cos_theta * wrench_world.fx + sin_theta * wrench_world.fy + wrench_body.fy = -sin_theta * wrench_world.fx + cos_theta * wrench_world.fy + wrench_body.tz = wrench_world.tz + + self.set_body_wrench(wrench_body) + + def clip_wrench(self, wrench: Wrench2D) -> Wrench2D: + wrench_clipped = Wrench2D() + force = np.sqrt(wrench.fx**2 + wrench.fy**2) + force_scale = min(self.p.actuators["F_body_max"] / force, 1.0) + torque_scale = min(self.p.actuators["M_body_max"] / abs(wrench.tz), 1.0) + + wrench_clipped.fx = wrench.fx * force_scale + wrench_clipped.fy = wrench.fy * force_scale + wrench_clipped.tz = wrench.tz * torque_scale + + return wrench_clipped diff --git a/ff_control/include/ff_control/linear_ctrl.hpp b/ff_control/include/ff_control/linear_ctrl.hpp index 91f3733..92ff80c 100644 --- a/ff_control/include/ff_control/linear_ctrl.hpp +++ b/ff_control/include/ff_control/linear_ctrl.hpp @@ -12,9 +12,11 @@ namespace ff { class LinearController : public WrenchController { public: - using StateVec = Eigen::Matrix; // [x, y, theta, vx, vy, wz] - using ControlVec = Eigen::Matrix; // [fx, fy, tz] - using FeedbackMat = Eigen::Matrix; + static constexpr int STATE_DIM = 6; + static constexpr int CONTROL_DIM = 3; + using StateVec = Eigen::Matrix; // [x, y, theta, vx, vy, wz] + using ControlVec = Eigen::Matrix; // [fx, fy, tz] + using FeedbackMat = Eigen::Matrix; LinearController(); diff --git a/ff_control/include/ff_control/ll_ctrl.hpp b/ff_control/include/ff_control/ll_ctrl.hpp index a125111..13152d5 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -32,7 +32,7 @@ class LowLevelController : virtual public rclcpp::Node { * @brief send command to set the inertial wheel velocity * @TODO(alvin): support this or remove? * - * @param velocity velocity in [m/s] + * @param velocity angular velocity in [rad/s] */ void SetWheelVelocity(const double& velocity); diff --git a/ff_control/scripts/ll_ctrl_test.py b/ff_control/scripts/ll_ctrl_test.py deleted file mode 100755 index 13e8c83..0000000 --- a/ff_control/scripts/ll_ctrl_test.py +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node - -import numpy as np - -from ff_control.ll_ctrl import LowLevelController - - -class TestLowLevelController(LowLevelController): - - def __init__(self): - super().__init__() - self.timer = self.create_timer(1.0, self.set_thrust_command) - - def set_thrust_command(self) -> None: - self.set_thrust_duty_cycle(np.ones(8) * 0.5) - - -def main(args=None): - rclpy.init(args=args) - debug_ctrl = TestLowLevelController() - rclpy.spin(debug_ctrl) - rclpy.shutdown() - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/ff_control/scripts/pd_ctrl_py_node b/ff_control/scripts/pd_ctrl_py_node new file mode 100755 index 0000000..ba637ca --- /dev/null +++ b/ff_control/scripts/pd_ctrl_py_node @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +import copy +import rclpy + +import numpy as np + +from ff_control.linear_ctrl import LinearController +from ff_msgs.msg import FreeFlyerStateStamped +from geometry_msgs.msg import PoseStamped + + +class PDControlNode(LinearController): + + def __init__(self): + super().__init__("pd_control_node") + self.state_sp_sub = self.create_subscription(FreeFlyerStateStamped, + "ctrl/state", self.state_setpoint_callback, 10) + self.rviz_sp_sub = self.create_subscription(PoseStamped, + "/goal_pose", self.rviz_setpoint_callback, 10) + self.timer = self.create_timer(0.1, self.control_loop) + self.state_desired = FreeFlyerStateStamped() + + # build feedback gain matrix + gain_f = self.declare_parameter("gain_f", 2.0) + gain_df = self.declare_parameter("gain_df", 10.0) + gain_t = self.declare_parameter("gain_t", 0.2) + gain_dt = self.declare_parameter("gain_dt", 0.4) + self.K = np.array([[gain_f, 0, 0, gain_df, 0, 0], + [0, gain_f, 0, 0, gain_df, 0], + [0, 0, gain_t, 0, 0, gain_dt]]) + + def state_ready_callback(self) -> None: + # copy current position as goal position + self.state_desired.header.stamp = self.get_clock().now().to_msg() + self.state_desired.state = self.get_state() + + def state_setpoint_callback(self, msg: FreeFlyerStateStamped) -> None: + self.state_desired = copy.deepcopy(msg) + + def rviz_setpoint_callback(self, msg: PoseStamped) -> None: + self.state_desired.header.stamp = msg.header.stamp + + self.state_desired.state.pose.x = msg->pose.position.x + self.state_desired.state.pose.y = msg->pose.position.y + z = msg->pose.orientation.z + w = msg->pose.orientation.w + self.state_desired.state.pose.theta = np.arctan2(2 * w * z, w * w - z * z) + + self.state_desired.state.twist.vx = 0 + self.state_desired.state.twist.vy = 0 + self.state_desired.state.twist.wz = 0 + + def control_loop(self) -> None: + # state not yet ready + if not self.state_is_ready(): + return + + self.send_control(self.state_desired.state, self.K) + +def main(args=None): + rclpy.init(args=args) + pd_ctrl = PDControlNode() + rclpy.spin(pd_ctrl) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ff_control/src/nodes/pd_ctrl_node.cpp b/ff_control/src/nodes/pd_ctrl_node.cpp index e15828b..0d09253 100644 --- a/ff_control/src/nodes/pd_ctrl_node.cpp +++ b/ff_control/src/nodes/pd_ctrl_node.cpp @@ -69,6 +69,8 @@ class PDControlNode : public ff::LinearController { } void GoalPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + state_des_.header.stamp = msg->header.stamp; + state_des_.state.pose.x = msg->pose.position.x; state_des_.state.pose.y = msg->pose.position.y; diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index 638242e..10072d1 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): ), Node( package="ff_control", - executable="key_teleop_node", + executable="key_teleop_cpp_node", name="key_teleop_node", namespace=robot_name, prefix="gnome-terminal --", diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 4fcb5ad..42ef820 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -1,5 +1,6 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -27,7 +28,7 @@ def generate_launch_description(): ), Node( package="ff_control", - executable="pd_ctrl_node", + executable="pd_ctrl_cpp_node", name="pd_ctrl_node", namespace=robot_name, ), From 1726120be8b8121f43ba97d2fdf9f098f60b3d6e Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 25 Apr 2023 14:23:44 -0700 Subject: [PATCH 03/56] python pd controller working --- ff_control/ff_control/linear_ctrl.py | 2 +- ff_control/ff_control/ll_ctrl.py | 4 +-- ff_control/ff_control/wrench_ctrl.py | 12 ++++----- ff_control/scripts/pd_ctrl_py_node | 38 ++++++++++++++++----------- ff_control/src/nodes/pd_ctrl_node.cpp | 37 ++++++++++++++------------ ff_control/src/wrench_ctrl.cpp | 10 +++---- freeflyer/launch/sim_pd.launch.py | 5 ++-- 7 files changed, 59 insertions(+), 49 deletions(-) diff --git a/ff_control/ff_control/linear_ctrl.py b/ff_control/ff_control/linear_ctrl.py index 49c2872..4f5588e 100644 --- a/ff_control/ff_control/linear_ctrl.py +++ b/ff_control/ff_control/linear_ctrl.py @@ -139,7 +139,7 @@ def state_is_ready(self) -> bool: return self._state_ready def _state_callback(self, msg: FreeFlyerStateStamped) -> None: - self._state_stamped = copy.deepcopy(msg.state) + self._state_stamped = copy.deepcopy(msg) if not self._state_ready: self._state_ready = True diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index 1df8317..6b9201d 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -17,8 +17,8 @@ def __init__(self, node_name: str = "ll_ctrl_node") -> None: self.p = RobotParams(self) # low level control publishers - self._thruster_pub = self.create_publisher(ThrusterCommand, "ctrl/duty_cycle", 10) - self._wheel_pub = self.create_publisher(WheelVelCommand, "ctrl/velocity", 10) + self._thruster_pub = self.create_publisher(ThrusterCommand, "commands/duty_cycle", 10) + self._wheel_pub = self.create_publisher(WheelVelCommand, "commands/velocity", 10) def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: """send command to set the thrusters duty cycles diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index a1f3e66..17490cd 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -44,7 +44,7 @@ def set_body_wrench(self, wrench_body: Wrench2D, use_wheel: bool = False) -> Non # convert torque u_M = wrench_body_clipped.tz / (4 * self.p.actuators["F_max_per_thruster"] \ - * self.p.actuators["thursters_lever_arm"]) + * self.p.actuators["thrusters_lever_arm"]) if u_M > 0: duty_cycle[[1, 3, 5, 7]] += u_M else: @@ -75,11 +75,11 @@ def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: def clip_wrench(self, wrench: Wrench2D) -> Wrench2D: wrench_clipped = Wrench2D() force = np.sqrt(wrench.fx**2 + wrench.fy**2) - force_scale = min(self.p.actuators["F_body_max"] / force, 1.0) - torque_scale = min(self.p.actuators["M_body_max"] / abs(wrench.tz), 1.0) + force_scale = max(force / self.p.actuators["F_body_max"], 1.0) + torque_scale = max(abs(wrench.tz) / self.p.actuators["M_body_max"], 1.0) - wrench_clipped.fx = wrench.fx * force_scale - wrench_clipped.fy = wrench.fy * force_scale - wrench_clipped.tz = wrench.tz * torque_scale + wrench_clipped.fx = wrench.fx / force_scale + wrench_clipped.fy = wrench.fy / force_scale + wrench_clipped.tz = wrench.tz / torque_scale return wrench_clipped diff --git a/ff_control/scripts/pd_ctrl_py_node b/ff_control/scripts/pd_ctrl_py_node index ba637ca..7e1745e 100755 --- a/ff_control/scripts/pd_ctrl_py_node +++ b/ff_control/scripts/pd_ctrl_py_node @@ -21,14 +21,11 @@ class PDControlNode(LinearController): self.timer = self.create_timer(0.1, self.control_loop) self.state_desired = FreeFlyerStateStamped() - # build feedback gain matrix - gain_f = self.declare_parameter("gain_f", 2.0) - gain_df = self.declare_parameter("gain_df", 10.0) - gain_t = self.declare_parameter("gain_t", 0.2) - gain_dt = self.declare_parameter("gain_dt", 0.4) - self.K = np.array([[gain_f, 0, 0, gain_df, 0, 0], - [0, gain_f, 0, 0, gain_df, 0], - [0, 0, gain_t, 0, 0, gain_dt]]) + # feedback gain params + self.declare_parameter("gain_f", 2.0) + self.declare_parameter("gain_df", 10.0) + self.declare_parameter("gain_t", 0.2) + self.declare_parameter("gain_dt", 0.4) def state_ready_callback(self) -> None: # copy current position as goal position @@ -41,22 +38,31 @@ class PDControlNode(LinearController): def rviz_setpoint_callback(self, msg: PoseStamped) -> None: self.state_desired.header.stamp = msg.header.stamp - self.state_desired.state.pose.x = msg->pose.position.x - self.state_desired.state.pose.y = msg->pose.position.y - z = msg->pose.orientation.z - w = msg->pose.orientation.w + self.state_desired.state.pose.x = msg.pose.position.x + self.state_desired.state.pose.y = msg.pose.position.y + z = msg.pose.orientation.z + w = msg.pose.orientation.w self.state_desired.state.pose.theta = np.arctan2(2 * w * z, w * w - z * z) - self.state_desired.state.twist.vx = 0 - self.state_desired.state.twist.vy = 0 - self.state_desired.state.twist.wz = 0 + self.state_desired.state.twist.vx = 0. + self.state_desired.state.twist.vy = 0. + self.state_desired.state.twist.wz = 0. def control_loop(self) -> None: # state not yet ready if not self.state_is_ready(): return - self.send_control(self.state_desired.state, self.K) + # build feedback gain matrix + gain_f = self.get_parameter("gain_f").get_parameter_value().double_value + gain_df = self.get_parameter("gain_df").get_parameter_value().double_value + gain_t = self.get_parameter("gain_t").get_parameter_value().double_value + gain_dt = self.get_parameter("gain_dt").get_parameter_value().double_value + K = np.array([[gain_f, 0, 0, gain_df, 0, 0], + [0, gain_f, 0, 0, gain_df, 0], + [0, 0, gain_t, 0, 0, gain_dt]]) + + self.send_control(self.state_desired.state, K) def main(args=None): rclpy.init(args=args) diff --git a/ff_control/src/nodes/pd_ctrl_node.cpp b/ff_control/src/nodes/pd_ctrl_node.cpp index 0d09253..b4102d3 100644 --- a/ff_control/src/nodes/pd_ctrl_node.cpp +++ b/ff_control/src/nodes/pd_ctrl_node.cpp @@ -20,26 +20,18 @@ class PDControlNode : public ff::LinearController { PDControlNode() : rclcpp::Node("pd_control_node"), ff::LinearController(), - state_des_{}, - K_(FeedbackMat::Zero()) { + state_des_{} { state_setpoint_sub_ = this->create_subscription( "ctrl/state", 10, std::bind(&PDControlNode::SetpointCallback, this, _1)); rviz_setpoint_sub_ = this->create_subscription( "/goal_pose", 10, std::bind(&PDControlNode::GoalPoseCallback, this, _1)); timer_ = this->create_wall_timer(100ms, std::bind(&PDControlNode::ControlLoop, this)); - const double gain_f = declare_parameter("gain_f", 2.0); - const double gain_df = declare_parameter("gain_df", 10.0); - const double gain_t = declare_parameter("gain_t", 0.2); - const double gain_dt = declare_parameter("gain_dt", 0.4); - - // construct feedback control matrix - K_(0, 0) = gain_f; - K_(0, 3) = gain_df; - K_(1, 1) = gain_f; - K_(1, 4) = gain_df; - K_(2, 2) = gain_t; - K_(2, 5) = gain_dt; + // default params for feedback gain matrix + this->declare_parameter("gain_f", 2.0); + this->declare_parameter("gain_df", 10.0); + this->declare_parameter("gain_t", 0.2); + this->declare_parameter("gain_dt", 0.4); } private: @@ -49,8 +41,6 @@ class PDControlNode : public ff::LinearController { ff_msgs::msg::FreeFlyerStateStamped state_des_; - FeedbackMat K_; - void StateReadyCallback() override { // copy current position as goal position state_des_.header.stamp = this->get_clock()->now(); @@ -61,7 +51,20 @@ class PDControlNode : public ff::LinearController { // state not yet ready if (!StateIsReady()) { return; } - SendControl(state_des_.state, K_); + // build feedback control matrix + const double gain_f = this->get_parameter("gain_f").as_double(); + const double gain_df = this->get_parameter("gain_df").as_double(); + const double gain_t = this->get_parameter("gain_t").as_double(); + const double gain_dt = this->get_parameter("gain_dt").as_double(); + FeedbackMat K = FeedbackMat::Zero(); + K(0, 0) = gain_f; + K(0, 3) = gain_df; + K(1, 1) = gain_f; + K(1, 4) = gain_df; + K(2, 2) = gain_t; + K(2, 5) = gain_dt; + + SendControl(state_des_.state, K); } void SetpointCallback(const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg) { diff --git a/ff_control/src/wrench_ctrl.cpp b/ff_control/src/wrench_ctrl.cpp index c896823..163533b 100644 --- a/ff_control/src/wrench_ctrl.cpp +++ b/ff_control/src/wrench_ctrl.cpp @@ -74,12 +74,12 @@ void WrenchController::SetWorldWrench(const ff_msgs::msg::Wrench2D& wrench_world Wrench2D WrenchController::ClipWrench(const Wrench2D& wrench) const { Wrench2D wrench_clipped; const double force = std::sqrt(wrench.fx * wrench.fx + wrench.fy * wrench.fy); - const double force_scale = std::min(p_.actuators.F_body_max / force, 1.0); - const double torque_scale = std::min(p_.actuators.M_body_max / std::abs(wrench.tz), 1.0); + const double force_scale = std::max(force / p_.actuators.F_body_max, 1.0); + const double torque_scale = std::max(std::abs(wrench.tz) / p_.actuators.M_body_max, 1.0); - wrench_clipped.fx = wrench.fx * force_scale; - wrench_clipped.fy = wrench.fy * force_scale; - wrench_clipped.tz = wrench.tz * torque_scale; + wrench_clipped.fx = wrench.fx / force_scale; + wrench_clipped.fy = wrench.fy / force_scale; + wrench_clipped.tz = wrench.tz / torque_scale; return wrench_clipped; } diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 42ef820..e30c0b4 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -1,15 +1,16 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition 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") + language = LaunchConfiguration("language") return LaunchDescription([ DeclareLaunchArgument("robot_name", default_value="robot"), + DeclareLaunchArgument("language", default_value="cpp", choices=["cpp", "py"]), IncludeLaunchDescription( PathJoinSubstitution([ FindPackageShare("ff_sim"), @@ -28,7 +29,7 @@ def generate_launch_description(): ), Node( package="ff_control", - executable="pd_ctrl_cpp_node", + executable=["pd_ctrl_", language, "_node"], name="pd_ctrl_node", namespace=robot_name, ), From 0e59af3bb698f5739b5c28937a26c4b12c39062c Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 25 Apr 2023 14:27:19 -0700 Subject: [PATCH 04/56] update cmake doc --- ff_control/CMakeLists.txt | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 2ac573b..5c59fe4 100644 --- a/ff_control/CMakeLists.txt +++ b/ff_control/CMakeLists.txt @@ -26,11 +26,13 @@ ament_target_dependencies(ctrl_lib rclcpp ff_msgs ff_params Eigen3) ament_export_targets(ctrl_libTarget HAS_LIBRARY_TARGET) ament_export_dependencies(rclcpp ff_msgs ff_params Eigen3 eigen3_cmake_module) +# install headers install( DIRECTORY include/ DESTINATION include ) +# install CPP libraries install( TARGETS ctrl_lib EXPORT ctrl_libTarget @@ -40,11 +42,6 @@ install( INCLUDES DESTINATION include ) -install(PROGRAMS - scripts/pd_ctrl_py_node - DESTINATION lib/${PROJECT_NAME} -) - add_executable(wrench_ctrl_cpp_node src/nodes/wrench_ctrl_node.cpp) target_link_libraries(wrench_ctrl_cpp_node ctrl_lib) @@ -55,11 +52,19 @@ ament_target_dependencies(pd_ctrl_cpp_node geometry_msgs) add_executable(key_teleop_cpp_node src/nodes/key_teleop_node.cpp) target_link_libraries(key_teleop_cpp_node ctrl_lib) +# install CPP nodes install(TARGETS wrench_ctrl_cpp_node pd_ctrl_cpp_node key_teleop_cpp_node DESTINATION lib/${PROJECT_NAME}) +# install Python libraries ament_python_install_package(${PROJECT_NAME}) +# install Python nodes +install(PROGRAMS + scripts/pd_ctrl_py_node + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights From bc9ececcaa3878791056084bd8bffff88c833785 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 25 Apr 2023 14:32:14 -0700 Subject: [PATCH 05/56] update readme --- freeflyer/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/freeflyer/README.md b/freeflyer/README.md index f3203cb..1dd8793 100644 --- a/freeflyer/README.md +++ b/freeflyer/README.md @@ -6,7 +6,8 @@ Top level package that launches multiple modules together. **PD Waypoint Control in Simulator** ```sh -ros2 launch freeflyer sim_pd.launch.py +ros2 launch freeflyer sim_pd.launch.py # launch CPP node by default +ros2 launch freeflyer sim_pd.launch.py language:=py # launch Python node ``` **Keyboard Teleop Velocity Control in Simulator** From 7f678025882d299a16b37dd6461e8226daebe111 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 25 Apr 2023 14:39:47 -0700 Subject: [PATCH 06/56] change launch argument name --- freeflyer/README.md | 4 ++-- freeflyer/launch/sim_pd.launch.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/freeflyer/README.md b/freeflyer/README.md index 1dd8793..bc5e06b 100644 --- a/freeflyer/README.md +++ b/freeflyer/README.md @@ -6,8 +6,8 @@ Top level package that launches multiple modules together. **PD Waypoint Control in Simulator** ```sh -ros2 launch freeflyer sim_pd.launch.py # launch CPP node by default -ros2 launch freeflyer sim_pd.launch.py language:=py # launch Python node +ros2 launch freeflyer sim_pd.launch.py # launch CPP implementation by default +ros2 launch freeflyer sim_pd.launch.py impl:=py # launch Python implementation ``` **Keyboard Teleop Velocity Control in Simulator** diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index e30c0b4..01e9288 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -6,11 +6,11 @@ def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") - language = LaunchConfiguration("language") + impl = LaunchConfiguration("impl") return LaunchDescription([ DeclareLaunchArgument("robot_name", default_value="robot"), - DeclareLaunchArgument("language", default_value="cpp", choices=["cpp", "py"]), + DeclareLaunchArgument("impl", default_value="cpp", choices=["cpp", "py"]), IncludeLaunchDescription( PathJoinSubstitution([ FindPackageShare("ff_sim"), @@ -29,7 +29,7 @@ def generate_launch_description(): ), Node( package="ff_control", - executable=["pd_ctrl_", language, "_node"], + executable=["pd_ctrl_", impl, "_node"], name="pd_ctrl_node", namespace=robot_name, ), From 0f88dc1a59063ce4377e12f9c51edadda3e86487 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 26 Apr 2023 22:31:13 -0700 Subject: [PATCH 07/56] add thruster driver --- .github/workflows/rolling.yml | 2 +- README.md | 1 + ff_drivers/CMakeLists.txt | 44 ++++ ff_drivers/README.md | 6 + ff_drivers/include/ff_drivers/pwm.hpp | 121 +++++++++ ff_drivers/launch/hardware_bringup.launch.py | 18 ++ ff_drivers/package.xml | 21 ++ ff_drivers/src/pwm.cpp | 252 +++++++++++++++++++ ff_drivers/src/test_single.cpp | 48 ++++ ff_drivers/src/thruster_node.cpp | 70 ++++++ 10 files changed, 582 insertions(+), 1 deletion(-) create mode 100644 ff_drivers/CMakeLists.txt create mode 100644 ff_drivers/README.md create mode 100644 ff_drivers/include/ff_drivers/pwm.hpp create mode 100644 ff_drivers/launch/hardware_bringup.launch.py create mode 100644 ff_drivers/package.xml create mode 100644 ff_drivers/src/pwm.cpp create mode 100644 ff_drivers/src/test_single.cpp create mode 100644 ff_drivers/src/thruster_node.cpp diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 8da13a4..6b805f9 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -8,7 +8,7 @@ on: jobs: build_docker: - runs-on: ubuntu-22.04 + runs-on: ubuntu-latest container: image: ubuntu:jammy steps: diff --git a/README.md b/README.md index 553246a..dc7eddf 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,7 @@ source ~/ff_ws/install/local_setup.bash * `freeflyer` -- top level pacakge (contains just launch files) * `ff_control` -- implement different controllers +* `ff_drivers` -- driver code that interfaces with hardware * `ff_msgs` -- custom message types * `ff_params` -- shared parameters about dynamics and actuators * `ff_sim` -- a lightweight pure Python simulator diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt new file mode 100644 index 0000000..3b6621b --- /dev/null +++ b/ff_drivers/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(ff_drivers) + +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(rclcpp REQUIRED) + +find_package(ff_msgs REQUIRED) + +add_library(pwm src/pwm.cpp) +target_include_directories(pwm PUBLIC + $ + $) +target_compile_features(pwm PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(pwm rclcpp) + +add_executable(thruster_node src/thruster_node.cpp) +target_link_libraries(thruster_node pwm) +ament_target_dependencies(thruster_node ff_msgs) + +add_executable(test_single src/test_single.cpp) +target_link_libraries(test_single pwm) + +# install nodes +install(TARGETS thruster_node test_single + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ff_drivers/README.md b/ff_drivers/README.md new file mode 100644 index 0000000..fd9a7bb --- /dev/null +++ b/ff_drivers/README.md @@ -0,0 +1,6 @@ +# ff\_drivers + +Driver code that interfaces with Freeflyer hardware such as thruster valves and the inertia wheel. +Designed around an embedded Linux device [Odroid N2L](https://wiki.odroid.com/odroid-n2l). + +## diff --git a/ff_drivers/include/ff_drivers/pwm.hpp b/ff_drivers/include/ff_drivers/pwm.hpp new file mode 100644 index 0000000..8c40cd9 --- /dev/null +++ b/ff_drivers/include/ff_drivers/pwm.hpp @@ -0,0 +1,121 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace ff { + +class PWM { + public: + virtual void High() {} + virtual void Low() {} + virtual bool IsSoft() const = 0; + virtual void Enable(); + virtual void Disable(); + virtual void SetPolarityNormal(); + virtual void SetPolarityInverse(); + virtual void SetPeriod(const std::chrono::duration& period); + virtual void SetDutyCycle(const std::chrono::duration& duty_cycle); + void SetDutyCyclePercent(double percent); + void Initialize(); + + private: + bool enabled_ = false; + bool polarity_inversed_ = false; + std::chrono::duration period_ = std::chrono::seconds(0); + std::chrono::duration duty_cycle_ = std::chrono::seconds(0); + + friend class PWMManager; +}; + +/** + * @brief hardware PWM control + * + * @see https://wiki.odroid.com/odroid-n2l/application_note/gpio/pwm#tab__odroid-n21 for pin maps + */ +class HardPWM : public PWM { + public: + enum PinDef { + PWM_C = 0, + PWM_D, + PWM_E, + PWM_F, + }; + + HardPWM(const PinDef& pwm); + ~HardPWM(); + + bool IsSoft() const override { return false; } + void Enable() override; + void Disable() override; + void SetPolarityNormal() override; + void SetPolarityInverse() override; + void SetPeriod(const std::chrono::duration& period) override; + void SetDutyCycle(const std::chrono::duration& duty_cycle) override; + + private: + std::string ChipBasePath() const; + std::string PWMBasePath() const; + + const PinDef pwm_; + std::ofstream f_enable_; + std::ofstream f_period_; + std::ofstream f_duty_cycle_; + std::ofstream f_polarity_; +}; + +/** + * @brief software PWM control with GPIO + * + * @see https://wiki.odroid.com/odroid-n2l/application_note/gpio/enhancement_40pins#tab__odroid-n2 + * for pin numbering + */ +class SoftPWM : public PWM { + public: + SoftPWM(int pin); + ~SoftPWM(); + + bool IsSoft() const override { return true; } + void High() override; + void Low() override; + void SetPolarityNormal() override; + void SetPolarityInverse() override; + + private: + int pin_; + std::ofstream f_value_; + std::ofstream f_active_low_; + + std::string GPIOBasePath() const; +}; + +class PWMManager : public rclcpp::Node { + public: + PWMManager(const std::string& node_name); + + void AddSoftPWM(int pin); + void AddHardPWM(const HardPWM::PinDef& pin); + + void EnableAll(); + void DisableAll(); + void SetPeriodAll(const std::chrono::duration& period); + void SetDutyCycle(size_t idx, double duty_cycle_percent); + + private: + // software PWM control + std::chrono::duration soft_period_; + rclcpp::TimerBase::SharedPtr period_timer_; + std::vector switch_timers_; + + std::vector> pwms_; + + void PeriodCallback(); + void SwitchCallback(size_t idx); +}; + +} // namespace ff diff --git a/ff_drivers/launch/hardware_bringup.launch.py b/ff_drivers/launch/hardware_bringup.launch.py new file mode 100644 index 0000000..d251fac --- /dev/null +++ b/ff_drivers/launch/hardware_bringup.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + robot_name = LaunchConfiguration("robot_name") + + return LaunchDescription([ + DeclareLaunchArgument("robot_name", default_value="robot"), + + Node( + package="ff_drivers", + executable="thruster_node", + name="thruster_node", + namespace=robot_name, + ), + ]) diff --git a/ff_drivers/package.xml b/ff_drivers/package.xml new file mode 100644 index 0000000..5d13b97 --- /dev/null +++ b/ff_drivers/package.xml @@ -0,0 +1,21 @@ + + + + ff_drivers + 0.0.0 + TODO: Package description + odroid + TODO: License declaration + + ament_cmake + + rclcpp + ff_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ff_drivers/src/pwm.cpp b/ff_drivers/src/pwm.cpp new file mode 100644 index 0000000..c92288f --- /dev/null +++ b/ff_drivers/src/pwm.cpp @@ -0,0 +1,252 @@ +#include "ff_drivers/pwm.hpp" + +namespace ff { + +void PWM::Enable() { + enabled_ = true; +} + +void PWM::Disable() { + enabled_ = false; +} + +void PWM::SetPolarityNormal() { + polarity_inversed_ = false; +} + +void PWM::SetPolarityInverse() { + polarity_inversed_ = true; +} + +void PWM::SetPeriod(const std::chrono::duration& period) { + period_ = period; +} + +void PWM::SetDutyCycle(const std::chrono::duration& duty_cycle) { + duty_cycle_ = duty_cycle; +} + +void PWM::SetDutyCyclePercent(double percent) { + SetDutyCycle(percent * period_); +} + +void PWM::Initialize() { + Disable(); + SetPolarityNormal(); + SetPeriod(period_); + SetDutyCycle(duty_cycle_); +} + +constexpr int pwmchip[] = { 4, 4, 8, 8 }; +constexpr int pwmpin[] = { 0, 1, 0, 1 }; + +HardPWM::HardPWM(const HardPWM::PinDef& pwm) : pwm_(pwm) { + // request device + { + std::ofstream f_export(ChipBasePath() + "export"); + f_export << pwmpin[pwm_]; + } + + f_enable_.open(PWMBasePath() + "enable"); + f_period_.open(PWMBasePath() + "period"); + f_duty_cycle_.open(PWMBasePath() + "duty_cycle"); + f_polarity_.open(PWMBasePath() + "polarity"); + + Initialize(); +} + +HardPWM::~HardPWM() { + Disable(); + + // close files + f_enable_.close(); + f_period_.close(); + f_duty_cycle_.close(); + f_polarity_.close(); + + // free device + std::ofstream f_unexport(ChipBasePath() + "unexport"); + f_unexport << pwmpin[pwm_]; +} + +void HardPWM::Enable() { + f_enable_ << 1; + f_enable_.flush(); + PWM::Enable(); +} + +void HardPWM::Disable() { + f_enable_ << 0; + f_enable_.flush(); + PWM::Disable(); +} + +void HardPWM::SetPolarityNormal() { + f_polarity_ << "normal"; + f_polarity_.flush(); + PWM::SetPolarityNormal(); +} + +void HardPWM::SetPolarityInverse() { + f_polarity_ << "inversed"; + f_polarity_.flush(); + PWM::SetPolarityInverse(); +} + +void HardPWM::SetPeriod(const std::chrono::duration& period) { + f_period_ << std::chrono::duration_cast(period).count(); + f_period_.flush(); + PWM::SetPeriod(period); +} + +void HardPWM::SetDutyCycle(const std::chrono::duration& duty_cycle) { + f_duty_cycle_ << std::chrono::duration_cast(duty_cycle).count(); + f_duty_cycle_.flush(); + PWM::SetDutyCycle(duty_cycle); +} + +std::string HardPWM::ChipBasePath() const { + return "/sys/class/pwm/pwmchip" + std::to_string(pwmchip[pwm_]) + "/"; +} + +std::string HardPWM::PWMBasePath() const { + return ChipBasePath() + "pwm" + std::to_string(pwmpin[pwm_]) + "/"; +} + +SoftPWM::SoftPWM(int pin) : pin_(pin) { + // request gpio device + { + std::ofstream f_export("/sys/class/gpio/export"); + f_export << pin; + } + + // configure GPIO to be an output + { + std::ofstream f_direction(GPIOBasePath() + "direction"); + f_direction << "out"; + } + + f_value_.open(GPIOBasePath() + "value"); + f_active_low_.open(GPIOBasePath() + "active_low"); + + Initialize(); +} + +SoftPWM::~SoftPWM() { + Disable(); + Low(); + + // close files + f_value_.close(); + f_active_low_.close(); + + // free gpio device + std::ofstream f_unexport("/sys/class/gpio/unexport"); + f_unexport << pin_; +} + +void SoftPWM::High() { + f_value_ << 1; + f_value_.flush(); +} + +void SoftPWM::Low() { + f_value_ << 0; + f_value_.flush(); +} + +void SoftPWM::SetPolarityNormal() { + f_active_low_ << 0; + f_active_low_.flush(); + PWM::SetPolarityNormal(); +} + +void SoftPWM::SetPolarityInverse() { + f_active_low_ << 1; + f_active_low_.flush(); + PWM::SetPolarityInverse(); +} + + +std::string SoftPWM::GPIOBasePath() const { + return "/sys/class/gpio/gpio" + std::to_string(pin_) + "/"; +} + +PWMManager::PWMManager(const std::string& node_name) : rclcpp::Node(node_name) {} + +void PWMManager::AddSoftPWM(int pin) { + pwms_.push_back(std::make_shared(pin)); + switch_timers_.push_back(nullptr); +} + +void PWMManager::AddHardPWM(const HardPWM::PinDef& pin) { + pwms_.push_back(std::make_shared(pin)); + switch_timers_.push_back(nullptr); +} + +void PWMManager::EnableAll() { + for (auto& pwm : pwms_) { + pwm->Enable(); + } +} + +void PWMManager::DisableAll() { + for (auto& pwm: pwms_) { + pwm->Disable(); + } +} + +void PWMManager::SetPeriodAll(const std::chrono::duration& period) { + for (auto& pwm: pwms_) { + pwm->SetPeriod(period); + } + + soft_period_ = period; + period_timer_ = this->create_wall_timer(period, std::bind(&PWMManager::PeriodCallback, this)); +} + +void PWMManager::SetDutyCycle(size_t idx, double duty_cycle_percent) { + if (idx >= pwms_.size()) { + RCLCPP_ERROR(this->get_logger(), "PWM idx out of range"); + return; + } + + if (duty_cycle_percent < 0 || duty_cycle_percent > 1) { + RCLCPP_WARN(this->get_logger(), "commanded duty cycle = %f clipped", duty_cycle_percent); + duty_cycle_percent = std::max(std::min(duty_cycle_percent, 1.), 0.); + } + + pwms_[idx]->SetDutyCyclePercent(duty_cycle_percent); +} + +void PWMManager::PeriodCallback() { + for (size_t i = 0; i < pwms_.size(); ++i) { + if (!pwms_[i]) { + RCLCPP_ERROR(this->get_logger(), "encountered unitialized PWM pointer"); + continue; + } + + if (pwms_[i]->IsSoft() && pwms_[i]->enabled_) { + pwms_[i]->High(); + switch_timers_[i] = this->create_wall_timer(pwms_[i]->duty_cycle_, + [this, i]() { SwitchCallback(i); }); + } + } +} + +void PWMManager::SwitchCallback(size_t idx) { + if (idx >= pwms_.size()) { + RCLCPP_ERROR(this->get_logger(), "PWM idx out of range"); + return; + } + + if (!pwms_[idx] || ! switch_timers_[idx]) { + RCLCPP_ERROR(this->get_logger(), "encountered unitialized pointer"); + return; + } + + switch_timers_[idx]->cancel(); + pwms_[idx]->Low(); +} + +} // namespace ff diff --git a/ff_drivers/src/test_single.cpp b/ff_drivers/src/test_single.cpp new file mode 100644 index 0000000..b148ca9 --- /dev/null +++ b/ff_drivers/src/test_single.cpp @@ -0,0 +1,48 @@ +#include + +#include + +#include "ff_drivers/pwm.hpp" + +using namespace std::chrono_literals; + + +class TestHardSingleNode : public ff::PWMManager { + public: + TestHardSingleNode() : ff::PWMManager("test_single_node") { + bool use_hard = this->declare_parameter("use_hard", true); + this->declare_parameter("max_duty_cycle", .2); + + if (use_hard) { + RCLCPP_INFO(this->get_logger(), "Using hardware PWM: PWM_C"); + this->AddHardPWM(ff::HardPWM::PWM_C); + } else { + RCLCPP_INFO(this->get_logger(), "Using software PWM: pin 477"); + this->AddSoftPWM(477); + } + + this->SetPeriodAll(100ms); + this->EnableAll(); + + timer_ = this->create_wall_timer(5s, [this]() { TimerCallback(); }); + } + + private: + void TimerCallback() { + double max_duty_cycle = this->get_parameter("max_duty_cycle").as_double(); + + this->SetDutyCycle(0, cnt_ * max_duty_cycle * .1); + RCLCPP_INFO(this->get_logger(), "Dutycycle set to %f", cnt_ * max_duty_cycle * .1); + cnt_ = (cnt_ + 1) % 11; + } + + int cnt_ = 0; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp new file mode 100644 index 0000000..a8bb397 --- /dev/null +++ b/ff_drivers/src/thruster_node.cpp @@ -0,0 +1,70 @@ +#include +#include + +#include + +#include "ff_drivers/pwm.hpp" +#include "ff_msgs/msg/thruster_command.hpp" + +#define NUM_THRUSTERS 8 + +// thruster pin connection +// @see: https://wiki.odroid.com/odroid-n2l/application_note/gpio/pwm#tab__odroid-n2 +static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { + 473, // thruster pin 1 -> odroid pin 7 + 479, // thruster pin 2 -> odroid pin 11 + 480, // thruster pin 3 -> odroid pin 13 + 490, // thruster pin 4 -> odroid pin 29 + 491, // thruster pin 5 -> odroid pin 31 + 476, // thruster pin 6 -> odroid pin 16 + 477, // thruster pin 7 -> odroid pin 18 + 478, // thruster pin 8 -> odroid pin 22 +}; + + +using namespace std::chrono_literals; +using ff_msgs::msg::ThrusterCommand; + +class ThrusterNode : public ff::PWMManager { + public: + ThrusterNode() : ff::PWMManager("thruster_driver_node") { + // add all PWMs + for (size_t i = 0; i < NUM_THRUSTERS; ++i) { + this->AddSoftPWM(THRUSTER_PINS[i]); + } + + // set period (default to 10Hz) + double period = this->declare_parameter("period", .1); + this->SetPeriodAll(period * 1s); + // update period on the fly + sub_params_ = std::make_shared(this); + cb_period_ = sub_params_->add_parameter_callback("period", + [this](const rclcpp::Parameter& p) { SetPeriodAll(p.as_double() * 1s); }); + + // start all PWMs + this->EnableAll(); + + // listen to commands + sub_duty_cycle_ = this->create_subscription("commands/duty_cycle", + 10, [this](const ThrusterCommand::SharedPtr msg) {DutyCycleCallback(msg);}); + } + + private: + std::shared_ptr sub_params_; + std::shared_ptr cb_period_; + rclcpp::Subscription::SharedPtr sub_duty_cycle_; + + void DutyCycleCallback(const ThrusterCommand::SharedPtr msg) { + for (size_t i = 0; i < NUM_THRUSTERS; ++i) { + this->SetDutyCycle(i, msg->duty_cycle[i]); + } + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From e655e0002c144098d03e53fd82dd6fed37689f9b Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 26 Apr 2023 22:36:02 -0700 Subject: [PATCH 08/56] forgot to install launch files for ff_drivers --- ff_drivers/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt index 3b6621b..5c30b9f 100644 --- a/ff_drivers/CMakeLists.txt +++ b/ff_drivers/CMakeLists.txt @@ -27,7 +27,11 @@ target_link_libraries(test_single pwm) # install nodes install(TARGETS thruster_node test_single - DESTINATION lib/${PROJECT_NAME}) + DESTINATION lib/${PROJECT_NAME}) + +# install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) From fef82f22f15e5904409b48f196c42bb3b37caeba Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 26 Apr 2023 23:13:21 -0700 Subject: [PATCH 09/56] update readme --- README.md | 5 +++-- ff_drivers/README.md | 21 ++++++++++++++++++++- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index dc7eddf..c99e749 100644 --- a/README.md +++ b/README.md @@ -18,10 +18,11 @@ git clone git@github.com:StanfordASL/freeflyer2.git ~/ff_ws/src/freeflyer2 ```sh rosdep update && rosdep install --from-paths ~/ff_ws/src --ignore-src -y ``` -4. Build the code +4. Build the code (skipping the driver package) ```sh -cd ~/ff_ws && colcon build +cd ~/ff_ws && colcon build --packages-skip ff_drivers ``` + 5. Source workspace install ```sh source ~/ff_ws/install/local_setup.bash diff --git a/ff_drivers/README.md b/ff_drivers/README.md index fd9a7bb..6337be7 100644 --- a/ff_drivers/README.md +++ b/ff_drivers/README.md @@ -3,4 +3,23 @@ Driver code that interfaces with Freeflyer hardware such as thruster valves and the inertia wheel. Designed around an embedded Linux device [Odroid N2L](https://wiki.odroid.com/odroid-n2l). -## +## Quick Start on ODROID +Follow the steps below to bring all the hardware actuators into ROS2 network. +1. Login to `root@` + + **Important Note**: we need to use the `root` account as it has + the right permission to access hardware interfaces. +2. Set up the freeflyer workspace `~/ff_ws` following steps 1 - 3 on the + [main setup guide](../) +3. Build the driver code +```sh +cd ~/ff_ws && colcon build --packages-up-to ff_drivers +``` +4. Source workspace install +```sh +source ~/ff_ws/install/local_setup.zsh +``` +5. Launch hardware bringup +```sh +ros2 launch ff_drivers hardware_bringup.launch.py +``` From 0ce2374c37eabc4d5cfc506106820a6a14ac0d5c Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 26 Apr 2023 23:15:29 -0700 Subject: [PATCH 10/56] drop foxy support --- .github/workflows/foxy.yml | 26 -------------------------- README.md | 5 ++--- 2 files changed, 2 insertions(+), 29 deletions(-) delete mode 100644 .github/workflows/foxy.yml diff --git a/.github/workflows/foxy.yml b/.github/workflows/foxy.yml deleted file mode 100644 index ae9639b..0000000 --- a/.github/workflows/foxy.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: foxy build - -on: - push: - branches: [ "main" ] - pull_request: - branches: [ "main" ] - -jobs: - build_docker: - runs-on: ubuntu-latest - container: - image: ubuntu:focal - steps: - - name: cancle preivious run - uses: styfle/cancel-workflow-action@0.11.0 - - name: setup ROS2 - uses: ros-tooling/setup-ros@v0.5 - with: - required-ros-distributions: foxy - - name: ROS2 build and test - uses: ros-tooling/action-ros-ci@v0.2 - with: - import-token: ${{ secrets.GITHUB_TOKEN }} - target-ros2-distro: foxy - skip-tests: true diff --git a/README.md b/README.md index c99e749..d934073 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,12 @@ # freeflyer2 -![foxy build](https://github.com/StanfordASL/freeflyer2/actions/workflows/foxy.yml/badge.svg?event=push) ![humble build](https://github.com/StanfordASL/freeflyer2/actions/workflows/humble.yml/badge.svg?event=push) ![rolling build](https://github.com/StanfordASL/freeflyer2/actions/workflows/rolling.yml/badge.svg?event=push) ASL FreeFlyer software stack with ROS2. See build status for support on ROS2 distrbutions. ## Quick Start -1. Install ROS2 ([Foxy](https://docs.ros.org/en/foxy/Installation.html), -[Humble](https://docs.ros.org/en/humble/Installation.html), or +1. Install ROS2 ( +[Humble](https://docs.ros.org/en/humble/Installation.html) or [Rolling](https://docs.ros.org/en/rolling/Installation.html)) 2. Clone the repo into a ROS2 workspace, e.g. ```sh From 0a570d15d7e5ae92f7af140d2fa72c133e9cbf4f Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 26 Apr 2023 23:19:01 -0700 Subject: [PATCH 11/56] fix readme relative link --- ff_drivers/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ff_drivers/README.md b/ff_drivers/README.md index 6337be7..7be46b7 100644 --- a/ff_drivers/README.md +++ b/ff_drivers/README.md @@ -10,7 +10,7 @@ Follow the steps below to bring all the hardware actuators into ROS2 network. **Important Note**: we need to use the `root` account as it has the right permission to access hardware interfaces. 2. Set up the freeflyer workspace `~/ff_ws` following steps 1 - 3 on the - [main setup guide](../) + [main setup guide](../README.md) 3. Build the driver code ```sh cd ~/ff_ws && colcon build --packages-up-to ff_drivers From 6451943be3c7b8d9b4b668002e485df5ca17b62a Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 28 Apr 2023 17:24:15 -0700 Subject: [PATCH 12/56] drivers working on hardware! --- ff_drivers/CMakeLists.txt | 9 +++- ff_drivers/launch/hardware_bringup.launch.py | 1 - .../launch/test_all_thrusters.launch.py | 23 +++++++++ ff_drivers/src/tests/test_all_thrusters.cpp | 51 +++++++++++++++++++ ff_drivers/src/{ => tests}/test_single.cpp | 11 ++-- ff_drivers/src/thruster_node.cpp | 4 +- 6 files changed, 89 insertions(+), 10 deletions(-) create mode 100644 ff_drivers/launch/test_all_thrusters.launch.py create mode 100644 ff_drivers/src/tests/test_all_thrusters.cpp rename ff_drivers/src/{ => tests}/test_single.cpp (75%) diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt index 5c30b9f..4d67542 100644 --- a/ff_drivers/CMakeLists.txt +++ b/ff_drivers/CMakeLists.txt @@ -18,15 +18,20 @@ target_include_directories(pwm PUBLIC target_compile_features(pwm PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 ament_target_dependencies(pwm rclcpp) +# nodes add_executable(thruster_node src/thruster_node.cpp) target_link_libraries(thruster_node pwm) ament_target_dependencies(thruster_node ff_msgs) -add_executable(test_single src/test_single.cpp) +# tests +add_executable(test_single src/tests/test_single.cpp) target_link_libraries(test_single pwm) +add_executable(test_all_thrusters src/tests/test_all_thrusters.cpp) +ament_target_dependencies(test_all_thrusters rclcpp ff_msgs) + # install nodes -install(TARGETS thruster_node test_single +install(TARGETS thruster_node test_single test_all_thrusters DESTINATION lib/${PROJECT_NAME}) # install launch files diff --git a/ff_drivers/launch/hardware_bringup.launch.py b/ff_drivers/launch/hardware_bringup.launch.py index d251fac..81abb15 100644 --- a/ff_drivers/launch/hardware_bringup.launch.py +++ b/ff_drivers/launch/hardware_bringup.launch.py @@ -8,7 +8,6 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("robot_name", default_value="robot"), - Node( package="ff_drivers", executable="thruster_node", diff --git a/ff_drivers/launch/test_all_thrusters.launch.py b/ff_drivers/launch/test_all_thrusters.launch.py new file mode 100644 index 0000000..68c179d --- /dev/null +++ b/ff_drivers/launch/test_all_thrusters.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + robot_name = LaunchConfiguration("robot_name") + + return LaunchDescription([ + DeclareLaunchArgument("robot_name", default_value="robot"), + Node( + package="ff_drivers", + executable="thruster_node", + name="thruster_node", + namespace=robot_name, + ), + Node( + package="ff_drivers", + executable="test_all_thrusters", + name="test_all_thrusters", + namespace=robot_name, + ) + ]) diff --git a/ff_drivers/src/tests/test_all_thrusters.cpp b/ff_drivers/src/tests/test_all_thrusters.cpp new file mode 100644 index 0000000..babee79 --- /dev/null +++ b/ff_drivers/src/tests/test_all_thrusters.cpp @@ -0,0 +1,51 @@ +#include + +#include + +#include "ff_msgs/msg/thruster_command.hpp" + +using namespace std::chrono_literals; +using ff_msgs::msg::ThrusterCommand; + + +class TestAllThrustersNode : public rclcpp::Node { + public: + TestAllThrustersNode() : rclcpp::Node("test_all_thrusters_node") { + thrust_cmd_pub_ = this->create_publisher("commands/duty_cycle", 10); + timer_ = this->create_wall_timer(5s, std::bind(&TestAllThrustersNode::TimerCallback, this)); + this->declare_parameter("duty_cycle", .2); + } + + private: + rclcpp::Publisher::SharedPtr thrust_cmd_pub_; + rclcpp::TimerBase::SharedPtr timer_; + int th_idx_ = 0; + + void TimerCallback() { + double duty_cycle = this->get_parameter("duty_cycle").as_double(); + + // populate thrust msg + ThrusterCommand msg{}; + for (int i = 0; i < 8; ++i) { + msg.duty_cycle[i] = 0.; + if (i == th_idx_) { + msg.duty_cycle[i] = duty_cycle; + } + } + + // publish thrust msg + this->thrust_cmd_pub_->publish(msg); + RCLCPP_INFO(this->get_logger(), "opening valve %d", th_idx_); + + // increment th_idx + th_idx_ = (th_idx_ + 1) % 8; + } +}; + + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ff_drivers/src/test_single.cpp b/ff_drivers/src/tests/test_single.cpp similarity index 75% rename from ff_drivers/src/test_single.cpp rename to ff_drivers/src/tests/test_single.cpp index b148ca9..67011e5 100644 --- a/ff_drivers/src/test_single.cpp +++ b/ff_drivers/src/tests/test_single.cpp @@ -7,18 +7,19 @@ using namespace std::chrono_literals; -class TestHardSingleNode : public ff::PWMManager { +class TestSingleNode : public ff::PWMManager { public: - TestHardSingleNode() : ff::PWMManager("test_single_node") { + TestSingleNode() : ff::PWMManager("test_single_node") { bool use_hard = this->declare_parameter("use_hard", true); + int pin = this->declare_parameter("pin", 473); this->declare_parameter("max_duty_cycle", .2); if (use_hard) { RCLCPP_INFO(this->get_logger(), "Using hardware PWM: PWM_C"); this->AddHardPWM(ff::HardPWM::PWM_C); } else { - RCLCPP_INFO(this->get_logger(), "Using software PWM: pin 477"); - this->AddSoftPWM(477); + RCLCPP_INFO(this->get_logger(), "Using software PWM: pin %d", pin); + this->AddSoftPWM(pin); } this->SetPeriodAll(100ms); @@ -42,7 +43,7 @@ class TestHardSingleNode : public ff::PWMManager { int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp index a8bb397..6f692f2 100644 --- a/ff_drivers/src/thruster_node.cpp +++ b/ff_drivers/src/thruster_node.cpp @@ -14,8 +14,8 @@ static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { 473, // thruster pin 1 -> odroid pin 7 479, // thruster pin 2 -> odroid pin 11 480, // thruster pin 3 -> odroid pin 13 - 490, // thruster pin 4 -> odroid pin 29 - 491, // thruster pin 5 -> odroid pin 31 + 472, // thruster pin 4 -> odroid pin 32 + 495, // thruster pin 5 -> odroid pin 36 476, // thruster pin 6 -> odroid pin 16 477, // thruster pin 7 -> odroid pin 18 478, // thruster pin 8 -> odroid pin 22 From 9e0b2c8dd8106d1a2e8bd9e8961f0e1d320cd7be Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 4 May 2023 23:51:18 -0700 Subject: [PATCH 13/56] launch param server with hardware_bringup --- ff_drivers/launch/hardware_bringup.launch.py | 6 ++++++ ff_drivers/package.xml | 2 ++ 2 files changed, 8 insertions(+) diff --git a/ff_drivers/launch/hardware_bringup.launch.py b/ff_drivers/launch/hardware_bringup.launch.py index 81abb15..cc2d589 100644 --- a/ff_drivers/launch/hardware_bringup.launch.py +++ b/ff_drivers/launch/hardware_bringup.launch.py @@ -8,6 +8,12 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("robot_name", default_value="robot"), + Node( + package="ff_params", + executable="robot_params_node", + name="robot_params_node", + namespace=robot_name, + ), Node( package="ff_drivers", executable="thruster_node", diff --git a/ff_drivers/package.xml b/ff_drivers/package.xml index 5d13b97..e619fc6 100644 --- a/ff_drivers/package.xml +++ b/ff_drivers/package.xml @@ -12,6 +12,8 @@ rclcpp ff_msgs + ff_params + ament_lint_auto ament_lint_common From d5be6424b89d98afda0b9f2bd851479caafd4db7 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 5 May 2023 13:18:05 -0700 Subject: [PATCH 14/56] estimation working with sim pd --- ff_control/package.xml | 1 + ff_control/src/linear_ctrl.cpp | 2 +- ff_estimate/CMakeLists.txt | 57 +++++++++++++++++++ .../include/ff_estimate/base_estimator.hpp | 23 ++++++++ .../include/ff_estimate/pose2d_estimator.hpp | 21 +++++++ ff_estimate/package.xml | 24 ++++++++ ff_estimate/src/base_estimator.cpp | 20 +++++++ ff_estimate/src/mocap_estimator_node.cpp | 43 ++++++++++++++ ff_estimate/src/pose2d_estimator.cpp | 50 ++++++++++++++++ ff_sim/ff_sim/simulator_node.py | 34 +++++++++-- ff_viz/rviz/default.rviz | 20 +++---- ff_viz/src/renderer_node.cpp | 6 +- freeflyer/launch/sim_pd.launch.py | 6 ++ 13 files changed, 287 insertions(+), 20 deletions(-) create mode 100644 ff_estimate/CMakeLists.txt create mode 100644 ff_estimate/include/ff_estimate/base_estimator.hpp create mode 100644 ff_estimate/include/ff_estimate/pose2d_estimator.hpp create mode 100644 ff_estimate/package.xml create mode 100644 ff_estimate/src/base_estimator.cpp create mode 100644 ff_estimate/src/mocap_estimator_node.cpp create mode 100644 ff_estimate/src/pose2d_estimator.cpp diff --git a/ff_control/package.xml b/ff_control/package.xml index e69c42f..0efcbd9 100644 --- a/ff_control/package.xml +++ b/ff_control/package.xml @@ -15,6 +15,7 @@ rclcpp geometry_msgs + ff_estimate ff_msgs ff_params diff --git a/ff_control/src/linear_ctrl.cpp b/ff_control/src/linear_ctrl.cpp index ed8ccb2..118a64d 100644 --- a/ff_control/src/linear_ctrl.cpp +++ b/ff_control/src/linear_ctrl.cpp @@ -33,7 +33,7 @@ LinearController::LinearController() : rclcpp::Node("linear_ctrl_node"), WrenchController() { state_sub_ = this->create_subscription( - "gt/state", 10, std::bind(&LinearController::StateCallback, this, _1)); + "est/state", 10, std::bind(&LinearController::StateCallback, this, _1)); } bool LinearController::StateIsReady() const { diff --git a/ff_estimate/CMakeLists.txt b/ff_estimate/CMakeLists.txt new file mode 100644 index 0000000..9513d2a --- /dev/null +++ b/ff_estimate/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) +project(ff_estimate) + +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(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) + +find_package(ff_msgs REQUIRED) + +add_library(est_lib src/base_estimator.cpp src/pose2d_estimator.cpp) +target_include_directories(est_lib PUBLIC + $ + $) +target_compile_features(est_lib PUBLIC cxx_std_17) +ament_target_dependencies(est_lib rclcpp ff_msgs geometry_msgs) + +ament_export_targets(est_libTarget HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp ff_msgs) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS est_lib + EXPORT est_libTarget + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +add_executable(mocap_estimator_node src/mocap_estimator_node.cpp) +target_link_libraries(mocap_estimator_node est_lib) + +install(TARGETS mocap_estimator_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ff_estimate/include/ff_estimate/base_estimator.hpp b/ff_estimate/include/ff_estimate/base_estimator.hpp new file mode 100644 index 0000000..553d9dd --- /dev/null +++ b/ff_estimate/include/ff_estimate/base_estimator.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +#include + +#include "ff_msgs/msg/free_flyer_state.hpp" +#include "ff_msgs/msg/free_flyer_state_stamped.hpp" + +namespace ff { + +class BaseEstimator : public rclcpp::Node { + public: + BaseEstimator(const std::string& node_name); + + protected: + void SendStateEstimate(const ff_msgs::msg::FreeFlyerState& state); + + private: + rclcpp::Publisher::SharedPtr state_pub_; +}; + +} // namespace ff diff --git a/ff_estimate/include/ff_estimate/pose2d_estimator.hpp b/ff_estimate/include/ff_estimate/pose2d_estimator.hpp new file mode 100644 index 0000000..b0bd2f7 --- /dev/null +++ b/ff_estimate/include/ff_estimate/pose2d_estimator.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "ff_estimate/base_estimator.hpp" +#include "ff_msgs/msg/free_flyer_state_stamped.hpp" +#include "ff_msgs/msg/pose2_d_stamped.hpp" + +namespace ff { + +class Pose2DEstimator: public BaseEstimator { + public: + Pose2DEstimator(const std::string& node_name); + + protected: + void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped& pose_stamped); + + private: + ff_msgs::msg::FreeFlyerStateStamped prev_; + bool prev_state_ready_ = false; +}; + +} // namespace ff diff --git a/ff_estimate/package.xml b/ff_estimate/package.xml new file mode 100644 index 0000000..d4deee7 --- /dev/null +++ b/ff_estimate/package.xml @@ -0,0 +1,24 @@ + + + + ff_estimate + 0.0.0 + TODO: Package description + alvin + TODO: License declaration + + ament_cmake + + rclcpp + geometry_msgs + ff_msgs + + vrpn_mocap + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ff_estimate/src/base_estimator.cpp b/ff_estimate/src/base_estimator.cpp new file mode 100644 index 0000000..d22c215 --- /dev/null +++ b/ff_estimate/src/base_estimator.cpp @@ -0,0 +1,20 @@ +#include "ff_estimate/base_estimator.hpp" + +using ff_msgs::msg::FreeFlyerState; +using ff_msgs::msg::FreeFlyerStateStamped; + +namespace ff { + +BaseEstimator::BaseEstimator(const std::string& node_name) : rclcpp::Node(node_name) { + state_pub_ = this->create_publisher("est/state", 10); +} + +void BaseEstimator::SendStateEstimate(const FreeFlyerState& state) { + ff_msgs::msg::FreeFlyerStateStamped msg{}; + msg.state = state; + msg.header.stamp = this->get_clock()->now(); + + state_pub_->publish(msg); +} + +} // namespace ff diff --git a/ff_estimate/src/mocap_estimator_node.cpp b/ff_estimate/src/mocap_estimator_node.cpp new file mode 100644 index 0000000..0bfd081 --- /dev/null +++ b/ff_estimate/src/mocap_estimator_node.cpp @@ -0,0 +1,43 @@ +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "ff_estimate/pose2d_estimator.hpp" +#include "ff_msgs/msg/pose2_d_stamped.hpp" + +using ff_msgs::msg::Pose2DStamped; +using geometry_msgs::msg::PoseStamped; + +class MocapEstimatorNode : public ff::Pose2DEstimator { + public: + MocapEstimatorNode() : ff::Pose2DEstimator("mocap_estimator_node") { + const std::string pose_channel = this->declare_parameter("pose_channel", "sim/mocap/pose"); + pose_sub_ = this->create_subscription( + pose_channel, 10, [this](const PoseStamped::SharedPtr msg) {PoseCallback(msg);}); + } + + private: + rclcpp::Subscription::SharedPtr pose_sub_; + + void PoseCallback(const PoseStamped::SharedPtr pose) { + // convert to pose2d + Pose2DStamped pose2d{}; + + pose2d.header = pose->header; + pose2d.pose.x = pose->pose.position.x; + pose2d.pose.y = pose->pose.position.y; + double w = pose->pose.orientation.w; + double z = pose->pose.orientation.z; + pose2d.pose.theta = std::atan2(2 * w * z, w * w - z * z); + + this->EstimateWithPose2D(pose2d); + } +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/ff_estimate/src/pose2d_estimator.cpp b/ff_estimate/src/pose2d_estimator.cpp new file mode 100644 index 0000000..119c2b1 --- /dev/null +++ b/ff_estimate/src/pose2d_estimator.cpp @@ -0,0 +1,50 @@ +#include "ff_estimate/pose2d_estimator.hpp" +#include "ff_msgs/msg/free_flyer_state.hpp" + +using ff_msgs::msg::FreeFlyerState; +using ff_msgs::msg::Pose2DStamped; + +namespace ff { + +Pose2DEstimator::Pose2DEstimator(const std::string& node_name) + : BaseEstimator(node_name) { + // perform simple 1st order IIR lowpass filter on derivate estimates + // coefficient in [0, 1) (higher coefficient filters better with larger delay) + this->declare_parameter("lowpass_coeff", .95); +} + +void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped& pose_stamped) { + FreeFlyerState state{}; + + state.pose = pose_stamped.pose; + if (prev_state_ready_) { + const rclcpp::Time now = pose_stamped.header.stamp; + const rclcpp::Time last = prev_.header.stamp; + double dt = (now - last).seconds(); + + // finite difference + double vx = (pose_stamped.pose.x - prev_.state.pose.x) / dt; + double vy = (pose_stamped.pose.y - prev_.state.pose.y) / dt; + // wrap angle delta to [-pi, pi] + double dtheta = std::remainder(pose_stamped.pose.theta - prev_.state.pose.theta, 2 * M_PI); + double wz = dtheta / dt; + + double alpha = this->get_parameter("lowpass_coeff").as_double(); + if (alpha < 0 || alpha >= 1) { + RCLCPP_ERROR(this->get_logger(), "IIR filter disabled: invalid coefficient %f", alpha); + alpha = 0; + } + state.twist.vx = alpha * prev_.state.twist.vx + (1. - alpha) * vx; + state.twist.vy = alpha * prev_.state.twist.vy + (1. - alpha) * vy; + state.twist.wz = alpha * prev_.state.twist.wz + (1. - alpha) * wz; + } else { + prev_state_ready_ = true; + } + + prev_.state = state; + prev_.header = pose_stamped.header; + + SendStateEstimate(state); +} + +} // namespace ff diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 99bfd40..15ce8bf 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -4,12 +4,14 @@ to a state evolution over time ----------------------------------------------- """ +import math import sys import numpy as np import typing as T import rclpy from rclpy.node import Node +from geometry_msgs.msg import PoseStamped from ff_msgs.msg import FreeFlyerStateStamped, Wrench2DStamped, ThrusterCommand, WheelVelCommand @@ -110,9 +112,14 @@ def __init__(self): "state_init", self.update_state_init_cb, 10) # ground truth publishers - self.pub_state = self.create_publisher(FreeFlyerStateStamped, f"gt/state", 10) - self.pub_wrench = self.create_publisher(Wrench2DStamped, f"gt/wrench", 10) - self.pub_ext_force = self.create_publisher(Wrench2DStamped, f"gt/ext_force", 10) + self.pub_state = self.create_publisher(FreeFlyerStateStamped, f"sim/state", 10) + self.pub_wrench = self.create_publisher(Wrench2DStamped, f"sim/wrench", 10) + self.pub_ext_force = self.create_publisher(Wrench2DStamped, f"sim/ext_force", 10) + + # simulated motion capture + self.declare_parameter("mocap_noise_xy", 0.001) + self.declare_parameter("mocap_noise_theta", math.radians(0.1)) + self.pub_mocap = self.create_publisher(PoseStamped, "sim/mocap/pose", 10) self.sim_timer = self.create_timer(self.SIM_DT, self.sim_loop) @@ -134,7 +141,7 @@ def sim_loop(self) -> None: F_worldFrame = np.matmul(R, F_bodyFrame) wrench_msg = Wrench2DStamped() wrench_msg.header.stamp = now - wrench_msg.header.frame_id = "map" + wrench_msg.header.frame_id = "world" wrench_msg.wrench.fx = F_worldFrame[0] wrench_msg.wrench.fy = F_worldFrame[1] wrench_msg.wrench.tz = M @@ -154,7 +161,7 @@ def sim_loop(self) -> None: wrench_ext_msg = Wrench2DStamped() wrench_ext_msg.header.stamp = now - wrench_ext_msg.header.frame_id = "map" + wrench_ext_msg.header.frame_id = "world" wrench_ext_msg.wrench.fx = W_contact[0] wrench_ext_msg.wrench.fy = W_contact[1] wrench_ext_msg.wrench.tz = W_contact[2] @@ -165,7 +172,7 @@ def sim_loop(self) -> None: # Save as a message state = FreeFlyerStateStamped() state.header.stamp = now - state.header.frame_id = "map" + state.header.frame_id = "world" state.state.pose.x = self.x_cur[0] state.state.pose.y = self.x_cur[1] state.state.pose.theta = self.x_cur[2] @@ -173,8 +180,23 @@ def sim_loop(self) -> None: state.state.twist.vy = self.x_cur[4] state.state.twist.wz = self.x_cur[5] + # simulated motion capture with noise + noise_xy = self.get_parameter("mocap_noise_xy").get_parameter_value().double_value + noise_theta = self.get_parameter("mocap_noise_theta").get_parameter_value().double_value + mocap = PoseStamped() + mocap.header.stamp = now + mocap.header.frame_id = "world" + mocap.pose.position.x = self.x_cur[0] + np.random.normal(loc=0., scale=noise_xy) + mocap.pose.position.y = self.x_cur[1] + np.random.normal(loc=0., scale=noise_xy) + theta = self.x_cur[2] + np.random.normal(loc=0., scale=noise_theta) + mocap.pose.orientation.w = np.cos(theta / 2) + mocap.pose.orientation.x = 0. + mocap.pose.orientation.y = 0. + mocap.pose.orientation.z = np.sin(theta / 2) + # Publish self.pub_state.publish(state) + self.pub_mocap.publish(mocap) def update_wheel_cmd_vel_cb(self, msg: WheelVelCommand) -> None: self.wheel_vel_cmd = msg.velocity diff --git a/ff_viz/rviz/default.rviz b/ff_viz/rviz/default.rviz index 7838172..4f90037 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: 870 + Tree Height: 637 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -100,17 +100,17 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - map: - Value: true robot/base: Value: true + world: + Value: true Marker Scale: 2.5 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - map: + world: robot/base: {} Update Interval: 0 @@ -118,7 +118,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 246; 245; 244 - Fixed Frame: map + Fixed Frame: world Frame Rate: 30 Name: root Tools: @@ -184,10 +184,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1376 + Height: 1022 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f70000045efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000045e0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000045efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e0000045e0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000005afc0100000002fb0000000800540069006d00650100000000000009b60000058100fffffffb0000000800540069006d00650100000000000004500000000000000000000006480000045e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f700000344fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000344000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000344fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000344000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007480000005afc0100000002fb0000000800540069006d0065010000000000000748000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e60000034400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -196,6 +196,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2486 - X: 148 - Y: 54 + Width: 1864 + X: 105 + Y: 270 diff --git a/ff_viz/src/renderer_node.cpp b/ff_viz/src/renderer_node.cpp index 9e883a4..eb8513d 100644 --- a/ff_viz/src/renderer_node.cpp +++ b/ff_viz/src/renderer_node.cpp @@ -30,7 +30,7 @@ class Renderer : public rclcpp::Node { marker_msg_.scale.x = 0.001; marker_msg_.scale.y = 0.001; marker_msg_.scale.z = 0.001; - marker_msg_.header.frame_id = "map"; + marker_msg_.header.frame_id = "world"; marker_msg_.action = visualization_msgs::msg::Marker::ADD; marker_msg_.pose.position.x = 0; marker_msg_.pose.position.y = 2.7; @@ -48,7 +48,7 @@ class Renderer : public rclcpp::Node { for (size_t i = 0; i < robot_names.size(); ++i) { // build pose messages transform_msgs_[i] = std::make_unique(); - transform_msgs_[i]->header.frame_id = "map"; + transform_msgs_[i]->header.frame_id = "world"; transform_msgs_[i]->child_frame_id = robot_names[i] + "/base"; transform_msgs_[i]->transform.rotation.x = 0.0; transform_msgs_[i]->transform.rotation.y = 0.0; @@ -60,7 +60,7 @@ class Renderer : public rclcpp::Node { // create pose subscription state_subs_[i] = this->create_subscription( - "/" + robot_names[i] + "/gt/state", + "/" + robot_names[i] + "/est/state", 10, [this, i](const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg){ this->StateCallback(i, msg); diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 4fcb5ad..5b4b357 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -31,4 +31,10 @@ def generate_launch_description(): name="pd_ctrl_node", namespace=robot_name, ), + Node( + package="ff_estimate", + executable="mocap_estimator_node", + name="mocap_estimator_node", + namespace=robot_name, + ), ]) From 9216c76b8b3f698258af4d0283f7eece33edc4cf Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 5 May 2023 14:59:50 -0700 Subject: [PATCH 15/56] add hardware launch --- freeflyer/launch/hardware_pd.launch.py | 39 ++++++++++++++++++++++++++ freeflyer/launch/mocap.launch.py | 17 +++++++++++ freeflyer/package.xml | 12 ++++---- 3 files changed, 63 insertions(+), 5 deletions(-) create mode 100644 freeflyer/launch/hardware_pd.launch.py create mode 100644 freeflyer/launch/mocap.launch.py diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py new file mode 100644 index 0000000..5ab1671 --- /dev/null +++ b/freeflyer/launch/hardware_pd.launch.py @@ -0,0 +1,39 @@ +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_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, + ), + Node( + package="ff_estimate", + executable="mocap_estimator_node", + name="mocap_estimator_node", + namespace=robot_name, + parameters=[{ + "pose_channel": PathJoinSubstitution([ + "/mocap", + robot_name, + "pose", + ]), + }], + ), + ]) diff --git a/freeflyer/launch/mocap.launch.py b/freeflyer/launch/mocap.launch.py new file mode 100644 index 0000000..09e6702 --- /dev/null +++ b/freeflyer/launch/mocap.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="vrpn_mocap", + executable="client_node", + name="vrpn_client_node", + namespace="mocap", + # ASL optitrack IP and port + parameters=[{ + "server": "192.168.1.8", + "port": 3883, + }], + ), + ]) diff --git a/freeflyer/package.xml b/freeflyer/package.xml index 42d347c..18f657c 100644 --- a/freeflyer/package.xml +++ b/freeflyer/package.xml @@ -9,11 +9,13 @@ ament_cmake - ff_control - ff_msgs - ff_params - ff_sim - ff_viz + vrpn_mocap + + ff_control + ff_msgs + ff_params + ff_sim + ff_viz ament_lint_auto ament_lint_common From 6748e02e41c8f943cf139dc238294065ebb4e8cf Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 5 May 2023 20:24:24 -0700 Subject: [PATCH 16/56] turnoff thruster and wheel_vel after destruction --- ff_control/include/ff_control/ll_ctrl.hpp | 1 + ff_control/src/ll_ctrl.cpp | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/ff_control/include/ff_control/ll_ctrl.hpp b/ff_control/include/ff_control/ll_ctrl.hpp index a125111..ab61f79 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -14,6 +14,7 @@ namespace ff { class LowLevelController : virtual public rclcpp::Node { public: LowLevelController(); + ~LowLevelController(); protected: /** diff --git a/ff_control/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index 6820def..d41a388 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -12,6 +12,13 @@ LowLevelController::LowLevelController() wheel_pub_ = this->create_publisher("commands/velocity", 10); } +LowLevelController::~LowLevelController() { + std::array thrust; + thrust.fill(0.); + SetThrustDutyCycle(thrust); + SetWheelVelocity(0.); +} + void LowLevelController::SetThrustDutyCycle(const std::array& duty_cycle) { ThrusterCommand msg{}; msg.header.stamp = this->get_clock()->now(); From 2a5b09310438454f8072ea56ea993e0ee92a5b6d Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Sun, 7 May 2023 23:10:33 -0700 Subject: [PATCH 17/56] add some docs --- ff_estimate/CMakeLists.txt | 2 ++ ff_estimate/include/ff_estimate/base_estimator.hpp | 8 ++++++++ ff_estimate/include/ff_estimate/pose2d_estimator.hpp | 10 +++++++++- ff_estimate/package.xml | 2 -- 4 files changed, 19 insertions(+), 3 deletions(-) diff --git a/ff_estimate/CMakeLists.txt b/ff_estimate/CMakeLists.txt index 9513d2a..d12685f 100644 --- a/ff_estimate/CMakeLists.txt +++ b/ff_estimate/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rclcpp REQUIRED) find_package(ff_msgs REQUIRED) +# Estimation library add_library(est_lib src/base_estimator.cpp src/pose2d_estimator.cpp) target_include_directories(est_lib PUBLIC $ @@ -36,6 +37,7 @@ install( INCLUDES DESTINATION include ) +# motion capture estimator node add_executable(mocap_estimator_node src/mocap_estimator_node.cpp) target_link_libraries(mocap_estimator_node est_lib) diff --git a/ff_estimate/include/ff_estimate/base_estimator.hpp b/ff_estimate/include/ff_estimate/base_estimator.hpp index 553d9dd..acdfd13 100644 --- a/ff_estimate/include/ff_estimate/base_estimator.hpp +++ b/ff_estimate/include/ff_estimate/base_estimator.hpp @@ -9,11 +9,19 @@ namespace ff { +/** + * @brief estimator base class + */ class BaseEstimator : public rclcpp::Node { public: BaseEstimator(const std::string& node_name); protected: + /** + * @brief send out the current state estimate + * + * @param state estimated freeflyer state + */ void SendStateEstimate(const ff_msgs::msg::FreeFlyerState& state); private: diff --git a/ff_estimate/include/ff_estimate/pose2d_estimator.hpp b/ff_estimate/include/ff_estimate/pose2d_estimator.hpp index b0bd2f7..250ce0f 100644 --- a/ff_estimate/include/ff_estimate/pose2d_estimator.hpp +++ b/ff_estimate/include/ff_estimate/pose2d_estimator.hpp @@ -6,12 +6,20 @@ namespace ff { +/** + * @brief full-state estimator using only pose measurements + */ class Pose2DEstimator: public BaseEstimator { public: Pose2DEstimator(const std::string& node_name); protected: - void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped& pose_stamped); + /** + * @brief estimate state with Pose2D + * + * @param pose_stamped timestamped pose2d + */ + virtual void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped& pose_stamped); private: ff_msgs::msg::FreeFlyerStateStamped prev_; diff --git a/ff_estimate/package.xml b/ff_estimate/package.xml index d4deee7..61b4fc9 100644 --- a/ff_estimate/package.xml +++ b/ff_estimate/package.xml @@ -13,8 +13,6 @@ geometry_msgs ff_msgs - vrpn_mocap - ament_lint_auto ament_lint_common From 589292b71000251002aa0cc44ccb328ae6d68992 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 11 May 2023 15:26:24 -0700 Subject: [PATCH 18/56] remove destructor cleanup as it does not work after rclcpp shutdown --- ff_control/include/ff_control/ll_ctrl.hpp | 1 - ff_control/include/ff_control/wrench_ctrl.hpp | 2 +- ff_control/src/ll_ctrl.cpp | 7 ------- 3 files changed, 1 insertion(+), 9 deletions(-) diff --git a/ff_control/include/ff_control/ll_ctrl.hpp b/ff_control/include/ff_control/ll_ctrl.hpp index ab61f79..a125111 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -14,7 +14,6 @@ namespace ff { class LowLevelController : virtual public rclcpp::Node { public: LowLevelController(); - ~LowLevelController(); protected: /** diff --git a/ff_control/include/ff_control/wrench_ctrl.hpp b/ff_control/include/ff_control/wrench_ctrl.hpp index 49fde4e..6566969 100644 --- a/ff_control/include/ff_control/wrench_ctrl.hpp +++ b/ff_control/include/ff_control/wrench_ctrl.hpp @@ -10,7 +10,7 @@ namespace ff { class WrenchController : public LowLevelController { public: - WrenchController(); + WrenchController(); protected: /** diff --git a/ff_control/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index d41a388..6820def 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -12,13 +12,6 @@ LowLevelController::LowLevelController() wheel_pub_ = this->create_publisher("commands/velocity", 10); } -LowLevelController::~LowLevelController() { - std::array thrust; - thrust.fill(0.); - SetThrustDutyCycle(thrust); - SetWheelVelocity(0.); -} - void LowLevelController::SetThrustDutyCycle(const std::array& duty_cycle) { ThrusterCommand msg{}; msg.header.stamp = this->get_clock()->now(); From e528438285d6c02322534180c61d3c5a1ad13d7d Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 11 May 2023 17:58:26 -0700 Subject: [PATCH 19/56] fix thruster allocation --- ff_control/src/wrench_ctrl.cpp | 20 +++++++------- ff_sim/ff_sim/simulator_node.py | 8 +++--- freeflyer/launch/estimation_viz.launch.py | 33 +++++++++++++++++++++++ 3 files changed, 47 insertions(+), 14 deletions(-) create mode 100644 freeflyer/launch/estimation_viz.launch.py diff --git a/ff_control/src/wrench_ctrl.cpp b/ff_control/src/wrench_ctrl.cpp index c896823..a3e4f98 100644 --- a/ff_control/src/wrench_ctrl.cpp +++ b/ff_control/src/wrench_ctrl.cpp @@ -24,29 +24,29 @@ void WrenchController::SetBodyWrench(const Wrench2D& wrench_body, bool use_wheel const double u_Fx = wrench_body_clipped.fx / (2 * p_.actuators.F_max_per_thruster); const double u_Fy = wrench_body_clipped.fy / (2 * p_.actuators.F_max_per_thruster); if (u_Fx > 0) { - duty_cycle[2] = u_Fx; - duty_cycle[5] = u_Fx; + duty_cycle[1] = u_Fx; + duty_cycle[6] = u_Fx; } else { - duty_cycle[1] = -u_Fx; - duty_cycle[6] = -u_Fx; + duty_cycle[2] = -u_Fx; + duty_cycle[5] = -u_Fx; } if (u_Fy > 0) { - duty_cycle[4] = u_Fy; - duty_cycle[7] = u_Fy; + duty_cycle[0] = u_Fy; + duty_cycle[3] = u_Fy; } else { - duty_cycle[0] = -u_Fy; - duty_cycle[3] = -u_Fy; + duty_cycle[4] = -u_Fy; + duty_cycle[7] = -u_Fy; } // convert torque const double u_M = wrench_body_clipped.tz / (4 * p_.actuators.F_max_per_thruster * p_.actuators.thrusters_lever_arm); if (u_M > 0) { - for (const int& i : {1, 3, 5, 7}) { + for (const int& i : {0, 2, 4, 6}) { duty_cycle[i] += u_M; } } else { - for (const int& i : {0, 2, 4, 6}) { + for (const int& i : {1, 3, 5, 7}) { duty_cycle[i] += -u_M; } } diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 15ce8bf..5bdee00 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -234,11 +234,11 @@ def thrusters_dutycycle_to_body_wrench(self, thrusters_dutycycles_vec): u_dc = thrusters_dutycycles_vec - F_x = Fmax * ( (u_dc[2]+u_dc[5]) - (u_dc[1]+u_dc[6]) ) - F_y = Fmax * ( (u_dc[4]+u_dc[7]) - (u_dc[0]+u_dc[3]) ) + F_x = Fmax * ( (u_dc[1]+u_dc[6]) - (u_dc[2]+u_dc[5]) ) + F_y = Fmax * ( (u_dc[0]+u_dc[3]) - (u_dc[4]+u_dc[7]) ) - M = (dist*Fmax) * ( (u_dc[1]+u_dc[3]+u_dc[5]+u_dc[7]) - - (u_dc[0]+u_dc[2]+u_dc[4]+u_dc[6]) ) + M = (dist*Fmax) * ( (u_dc[0]+u_dc[2]+u_dc[4]+u_dc[6]) - + (u_dc[1]+u_dc[3]+u_dc[5]+u_dc[7]) ) return np.array([F_x, F_y]), M diff --git a/freeflyer/launch/estimation_viz.launch.py b/freeflyer/launch/estimation_viz.launch.py new file mode 100644 index 0000000..92f866e --- /dev/null +++ b/freeflyer/launch/estimation_viz.launch.py @@ -0,0 +1,33 @@ +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_viz"), + "launch", + "ff_viz.launch.py", + ]), + launch_arguments={"robot_name": robot_name}.items(), + ), + Node( + package="ff_estimate", + executable="mocap_estimator_node", + name="mocap_estimator_node", + namespace=robot_name, + parameters=[{ + "pose_channel": PathJoinSubstitution([ + "/mocap", + robot_name, + "pose", + ]), + }], + ), + ]) From 52060934ef87df7c80c855e7f81c6021dc72b6dd Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 11 May 2023 18:06:13 -0700 Subject: [PATCH 20/56] add vrpn to hardware bringupg --- ff_drivers/launch/hardware_bringup.launch.py | 13 ++++++++++++- ff_drivers/package.xml | 1 + ff_estimate/src/mocap_estimator_node.cpp | 2 +- ff_sim/ff_sim/simulator_node.py | 2 +- freeflyer/launch/hardware_pd.launch.py | 18 +++++++++--------- 5 files changed, 24 insertions(+), 12 deletions(-) diff --git a/ff_drivers/launch/hardware_bringup.launch.py b/ff_drivers/launch/hardware_bringup.launch.py index cc2d589..bcd1f62 100644 --- a/ff_drivers/launch/hardware_bringup.launch.py +++ b/ff_drivers/launch/hardware_bringup.launch.py @@ -1,6 +1,6 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node def generate_launch_description(): @@ -20,4 +20,15 @@ def generate_launch_description(): name="thruster_node", namespace=robot_name, ), + Node( + package="vrpn_mocap", + executable="client_node", + name="vrpn_client_node", + namespace=PathJoinSubstitution([robot_name, "mocap"]), + # ASL optitrack IP and port + parameters=[{ + "server": "192.168.1.8", + "port": 3883, + }], + ), ]) diff --git a/ff_drivers/package.xml b/ff_drivers/package.xml index e619fc6..944634d 100644 --- a/ff_drivers/package.xml +++ b/ff_drivers/package.xml @@ -13,6 +13,7 @@ ff_msgs ff_params + vrpn_mocap ament_lint_auto ament_lint_common diff --git a/ff_estimate/src/mocap_estimator_node.cpp b/ff_estimate/src/mocap_estimator_node.cpp index 0bfd081..00bb363 100644 --- a/ff_estimate/src/mocap_estimator_node.cpp +++ b/ff_estimate/src/mocap_estimator_node.cpp @@ -13,7 +13,7 @@ using geometry_msgs::msg::PoseStamped; class MocapEstimatorNode : public ff::Pose2DEstimator { public: MocapEstimatorNode() : ff::Pose2DEstimator("mocap_estimator_node") { - const std::string pose_channel = this->declare_parameter("pose_channel", "sim/mocap/pose"); + const std::string pose_channel = this->declare_parameter("pose_channel", "mocap/sim/pose"); pose_sub_ = this->create_subscription( pose_channel, 10, [this](const PoseStamped::SharedPtr msg) {PoseCallback(msg);}); } diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 5bdee00..6829afa 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -119,7 +119,7 @@ def __init__(self): # simulated motion capture self.declare_parameter("mocap_noise_xy", 0.001) self.declare_parameter("mocap_noise_theta", math.radians(0.1)) - self.pub_mocap = self.create_publisher(PoseStamped, "sim/mocap/pose", 10) + self.pub_mocap = self.create_publisher(PoseStamped, "mocap/sim/pose", 10) self.sim_timer = self.create_timer(self.SIM_DT, self.sim_loop) diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index 5ab1671..d16459d 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -9,14 +9,14 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("robot_name", default_value="robot"), - IncludeLaunchDescription( - PathJoinSubstitution([ - FindPackageShare("ff_viz"), - "launch", - "ff_viz.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", @@ -30,7 +30,7 @@ def generate_launch_description(): namespace=robot_name, parameters=[{ "pose_channel": PathJoinSubstitution([ - "/mocap", + "mocap", robot_name, "pose", ]), From 8d199db57ddcc8c6254d564593a2a05f05be655a Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 11 May 2023 18:09:45 -0700 Subject: [PATCH 21/56] fix namespace --- freeflyer/launch/estimation_viz.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/freeflyer/launch/estimation_viz.launch.py b/freeflyer/launch/estimation_viz.launch.py index 92f866e..8babe9e 100644 --- a/freeflyer/launch/estimation_viz.launch.py +++ b/freeflyer/launch/estimation_viz.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): namespace=robot_name, parameters=[{ "pose_channel": PathJoinSubstitution([ - "/mocap", + "mocap", robot_name, "pose", ]), From 86d536d9111eb936af33f2929eddd8cc291331b9 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 12 May 2023 16:40:05 -0700 Subject: [PATCH 22/56] fix velocity estimator --- ff_control/src/wrench_ctrl.cpp | 20 ++++++++++---------- ff_estimate/src/pose2d_estimator.cpp | 6 ++++++ ff_sim/ff_sim/simulator_node.py | 8 ++++---- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/ff_control/src/wrench_ctrl.cpp b/ff_control/src/wrench_ctrl.cpp index a3e4f98..c896823 100644 --- a/ff_control/src/wrench_ctrl.cpp +++ b/ff_control/src/wrench_ctrl.cpp @@ -24,29 +24,29 @@ void WrenchController::SetBodyWrench(const Wrench2D& wrench_body, bool use_wheel const double u_Fx = wrench_body_clipped.fx / (2 * p_.actuators.F_max_per_thruster); const double u_Fy = wrench_body_clipped.fy / (2 * p_.actuators.F_max_per_thruster); if (u_Fx > 0) { - duty_cycle[1] = u_Fx; - duty_cycle[6] = u_Fx; + duty_cycle[2] = u_Fx; + duty_cycle[5] = u_Fx; } else { - duty_cycle[2] = -u_Fx; - duty_cycle[5] = -u_Fx; + duty_cycle[1] = -u_Fx; + duty_cycle[6] = -u_Fx; } if (u_Fy > 0) { - duty_cycle[0] = u_Fy; - duty_cycle[3] = u_Fy; + duty_cycle[4] = u_Fy; + duty_cycle[7] = u_Fy; } else { - duty_cycle[4] = -u_Fy; - duty_cycle[7] = -u_Fy; + duty_cycle[0] = -u_Fy; + duty_cycle[3] = -u_Fy; } // convert torque const double u_M = wrench_body_clipped.tz / (4 * p_.actuators.F_max_per_thruster * p_.actuators.thrusters_lever_arm); if (u_M > 0) { - for (const int& i : {0, 2, 4, 6}) { + for (const int& i : {1, 3, 5, 7}) { duty_cycle[i] += u_M; } } else { - for (const int& i : {1, 3, 5, 7}) { + for (const int& i : {0, 2, 4, 6}) { duty_cycle[i] += -u_M; } } diff --git a/ff_estimate/src/pose2d_estimator.cpp b/ff_estimate/src/pose2d_estimator.cpp index 119c2b1..6f4e297 100644 --- a/ff_estimate/src/pose2d_estimator.cpp +++ b/ff_estimate/src/pose2d_estimator.cpp @@ -11,6 +11,7 @@ Pose2DEstimator::Pose2DEstimator(const std::string& node_name) // perform simple 1st order IIR lowpass filter on derivate estimates // coefficient in [0, 1) (higher coefficient filters better with larger delay) this->declare_parameter("lowpass_coeff", .95); + this->declare_parameter("min_dt", 0.005); } void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped& pose_stamped) { @@ -22,6 +23,11 @@ void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped& pose_stamped) { const rclcpp::Time last = prev_.header.stamp; double dt = (now - last).seconds(); + // ignore this frame if it is too close to the last frame + if (dt < this->get_parameter("min_dt").as_double()) { + return; + } + // finite difference double vx = (pose_stamped.pose.x - prev_.state.pose.x) / dt; double vy = (pose_stamped.pose.y - prev_.state.pose.y) / dt; diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 6829afa..8091ddc 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -234,11 +234,11 @@ def thrusters_dutycycle_to_body_wrench(self, thrusters_dutycycles_vec): u_dc = thrusters_dutycycles_vec - F_x = Fmax * ( (u_dc[1]+u_dc[6]) - (u_dc[2]+u_dc[5]) ) - F_y = Fmax * ( (u_dc[0]+u_dc[3]) - (u_dc[4]+u_dc[7]) ) + F_x = Fmax * ( (u_dc[2]+u_dc[5]) - (u_dc[1]+u_dc[6]) ) + F_y = Fmax * ( (u_dc[4]+u_dc[7]) - (u_dc[0]+u_dc[3]) ) - M = (dist*Fmax) * ( (u_dc[0]+u_dc[2]+u_dc[4]+u_dc[6]) - - (u_dc[1]+u_dc[3]+u_dc[5]+u_dc[7]) ) + M = (dist*Fmax) * ( (u_dc[1]+u_dc[3]+u_dc[5]+u_dc[7]) - + (u_dc[0]+u_dc[2]+u_dc[4]+u_dc[6]) ) return np.array([F_x, F_y]), M From b33b992a34862d56f4bfc78fa19ff82a44c44ab9 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 16 May 2023 16:09:18 -0700 Subject: [PATCH 23/56] pull changes from driver branch --- ff_estimate/src/mocap_estimator_node.cpp | 2 +- ff_estimate/src/pose2d_estimator.cpp | 6 +++++ freeflyer/launch/estimation_viz.launch.py | 33 +++++++++++++++++++++++ 3 files changed, 40 insertions(+), 1 deletion(-) create mode 100644 freeflyer/launch/estimation_viz.launch.py diff --git a/ff_estimate/src/mocap_estimator_node.cpp b/ff_estimate/src/mocap_estimator_node.cpp index 0bfd081..00bb363 100644 --- a/ff_estimate/src/mocap_estimator_node.cpp +++ b/ff_estimate/src/mocap_estimator_node.cpp @@ -13,7 +13,7 @@ using geometry_msgs::msg::PoseStamped; class MocapEstimatorNode : public ff::Pose2DEstimator { public: MocapEstimatorNode() : ff::Pose2DEstimator("mocap_estimator_node") { - const std::string pose_channel = this->declare_parameter("pose_channel", "sim/mocap/pose"); + const std::string pose_channel = this->declare_parameter("pose_channel", "mocap/sim/pose"); pose_sub_ = this->create_subscription( pose_channel, 10, [this](const PoseStamped::SharedPtr msg) {PoseCallback(msg);}); } diff --git a/ff_estimate/src/pose2d_estimator.cpp b/ff_estimate/src/pose2d_estimator.cpp index 119c2b1..6f4e297 100644 --- a/ff_estimate/src/pose2d_estimator.cpp +++ b/ff_estimate/src/pose2d_estimator.cpp @@ -11,6 +11,7 @@ Pose2DEstimator::Pose2DEstimator(const std::string& node_name) // perform simple 1st order IIR lowpass filter on derivate estimates // coefficient in [0, 1) (higher coefficient filters better with larger delay) this->declare_parameter("lowpass_coeff", .95); + this->declare_parameter("min_dt", 0.005); } void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped& pose_stamped) { @@ -22,6 +23,11 @@ void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped& pose_stamped) { const rclcpp::Time last = prev_.header.stamp; double dt = (now - last).seconds(); + // ignore this frame if it is too close to the last frame + if (dt < this->get_parameter("min_dt").as_double()) { + return; + } + // finite difference double vx = (pose_stamped.pose.x - prev_.state.pose.x) / dt; double vy = (pose_stamped.pose.y - prev_.state.pose.y) / dt; diff --git a/freeflyer/launch/estimation_viz.launch.py b/freeflyer/launch/estimation_viz.launch.py new file mode 100644 index 0000000..8babe9e --- /dev/null +++ b/freeflyer/launch/estimation_viz.launch.py @@ -0,0 +1,33 @@ +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_viz"), + "launch", + "ff_viz.launch.py", + ]), + launch_arguments={"robot_name": robot_name}.items(), + ), + Node( + package="ff_estimate", + executable="mocap_estimator_node", + name="mocap_estimator_node", + namespace=robot_name, + parameters=[{ + "pose_channel": PathJoinSubstitution([ + "mocap", + robot_name, + "pose", + ]), + }], + ), + ]) From 51f7962e7bd0eaa42ea804c94b4b214da6ea5185 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 16 May 2023 16:12:46 -0700 Subject: [PATCH 24/56] update package --- freeflyer/package.xml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/freeflyer/package.xml b/freeflyer/package.xml index 42d347c..cb779fa 100644 --- a/freeflyer/package.xml +++ b/freeflyer/package.xml @@ -9,11 +9,12 @@ ament_cmake - ff_control - ff_msgs - ff_params - ff_sim - ff_viz + ff_estimate + ff_control + ff_msgs + ff_params + ff_sim + ff_viz ament_lint_auto ament_lint_common From 0ae3d9adcd14569466df2c1c080b940234da0ae4 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 16 May 2023 16:17:34 -0700 Subject: [PATCH 25/56] update readme --- freeflyer/README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/freeflyer/README.md b/freeflyer/README.md index f3203cb..f6a497f 100644 --- a/freeflyer/README.md +++ b/freeflyer/README.md @@ -2,7 +2,7 @@ Top level package that launches multiple modules together. -### Quick Examples +### Launch Files **PD Waypoint Control in Simulator** ```sh @@ -14,3 +14,11 @@ ros2 launch freeflyer sim_pd.launch.py ros2 launch freeflyer sim_key_teleop.launch.py ``` *Note*: this will launch a new terminal window where you can input keyboard commands + +**Finite Difference State Estimation with RVIZ Visuliazation** + +The following script is useful for debugging connections between hardware freeflyer and +the motion capture system. +```sh +ros2 launch freeflyer estimation_viz.launch.py +``` From 0e27c9c4b0ec1aa32c3b4704cc86561360d892d4 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 16 May 2023 16:23:53 -0700 Subject: [PATCH 26/56] update readme --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 553246a..e944f5d 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,7 @@ source ~/ff_ws/install/local_setup.bash ## Package Layout * `freeflyer` -- top level pacakge (contains just launch files) +* `ff_estimate` -- implement different state estimators * `ff_control` -- implement different controllers * `ff_msgs` -- custom message types * `ff_params` -- shared parameters about dynamics and actuators From b6388399ef6a17be4f258150e550a4a693556a18 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 10:33:39 -0700 Subject: [PATCH 27/56] sync sim changes with driver branch and add estimator to sim_key_teleop launch --- ff_sim/ff_sim/simulator_node.py | 2 +- freeflyer/launch/sim_key_teleop.launch.py | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 15ce8bf..8091ddc 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -119,7 +119,7 @@ def __init__(self): # simulated motion capture self.declare_parameter("mocap_noise_xy", 0.001) self.declare_parameter("mocap_noise_theta", math.radians(0.1)) - self.pub_mocap = self.create_publisher(PoseStamped, "sim/mocap/pose", 10) + self.pub_mocap = self.create_publisher(PoseStamped, "mocap/sim/pose", 10) self.sim_timer = self.create_timer(self.SIM_DT, self.sim_loop) diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index 638242e..79076ee 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -32,4 +32,10 @@ def generate_launch_description(): namespace=robot_name, prefix="gnome-terminal --", ), + Node( + package="ff_estimate", + executable="mocap_estimator_node", + name="mocap_estimator_node", + namespace=robot_name, + ), ]) From c77ba03820305c3b1dfbc8765d743598e0cd9c7d Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 11:23:28 -0700 Subject: [PATCH 28/56] fix style for ff_control --- ff_control/CMakeLists.txt | 7 -- .../include/ff_control/keyboard_ctrl.hpp | 40 +++++++-- ff_control/include/ff_control/linear_ctrl.hpp | 52 ++++++++--- ff_control/include/ff_control/ll_ctrl.hpp | 41 +++++++-- ff_control/include/ff_control/wrench_ctrl.hpp | 45 +++++++--- ff_control/src/keyboard_ctrl.cpp | 40 +++++++-- ff_control/src/linear_ctrl.cpp | 61 ++++++++++--- ff_control/src/ll_ctrl.cpp | 39 +++++++-- ff_control/src/nodes/key_teleop_node.cpp | 86 ++++++++++++------- ff_control/src/nodes/pd_ctrl_node.cpp | 56 +++++++++--- ff_control/src/nodes/wrench_ctrl_node.cpp | 47 ++++++++-- ff_control/src/wrench_ctrl.cpp | 45 ++++++++-- 12 files changed, 427 insertions(+), 132 deletions(-) diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 50f6b46..d1e07cc 100644 --- a/ff_control/CMakeLists.txt +++ b/ff_control/CMakeLists.txt @@ -54,13 +54,6 @@ install(TARGETS wrench_ctrl_node pd_ctrl_node key_teleop_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_control/include/ff_control/keyboard_ctrl.hpp b/ff_control/include/ff_control/keyboard_ctrl.hpp index 048c243..745fe9e 100644 --- a/ff_control/include/ff_control/keyboard_ctrl.hpp +++ b/ff_control/include/ff_control/keyboard_ctrl.hpp @@ -1,18 +1,44 @@ +// 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. + + #pragma once -#include #include +#include + #include -namespace ff { +namespace ff +{ -class KeyboardController : virtual public rclcpp::Node { - public: +class KeyboardController : virtual public rclcpp::Node +{ +public: KeyboardController(); ~KeyboardController(); - protected: +protected: /** * @brief get the last key press * @@ -20,7 +46,7 @@ class KeyboardController : virtual public rclcpp::Node { */ char GetKey(); - private: +private: // termio settings backup struct termios old_settings_; int f_flags_; @@ -34,4 +60,4 @@ class KeyboardController : virtual public rclcpp::Node { void KeyUpdate(); }; -} // namespace ff +} // namespace ff diff --git a/ff_control/include/ff_control/linear_ctrl.hpp b/ff_control/include/ff_control/linear_ctrl.hpp index 91f3733..9ca1562 100644 --- a/ff_control/include/ff_control/linear_ctrl.hpp +++ b/ff_control/include/ff_control/linear_ctrl.hpp @@ -1,24 +1,48 @@ +// 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. + + #pragma once -#include "ff_control/wrench_ctrl.hpp" +#include +#include +#include "ff_control/wrench_ctrl.hpp" #include "ff_msgs/msg/free_flyer_state.hpp" #include "ff_msgs/msg/free_flyer_state_stamped.hpp" -#include -#include - -namespace ff { +namespace ff +{ -class LinearController : public WrenchController { - public: +class LinearController : public WrenchController +{ +public: using StateVec = Eigen::Matrix; // [x, y, theta, vx, vy, wz] using ControlVec = Eigen::Matrix; // [fx, fy, tz] using FeedbackMat = Eigen::Matrix; LinearController(); - protected: +protected: /** * @brief check whether the state is available * @@ -33,7 +57,7 @@ class LinearController : public WrenchController { * * @return true if state is ready */ - bool GetState(StateVec* state) const; + bool GetState(StateVec * state) const; /** * @brief get the current state of the robot @@ -42,7 +66,7 @@ class LinearController : public WrenchController { * * @return true if state is ready */ - bool GetState(ff_msgs::msg::FreeFlyerState* state) const; + bool GetState(ff_msgs::msg::FreeFlyerState * state) const; /** * @brief send linear feedback control command @@ -50,7 +74,7 @@ class LinearController : public WrenchController { * @param state_des desired state * @param K feedback control matrix (i.e. u = Kx) */ - void SendControl(const StateVec& state_des, const FeedbackMat& K); + void SendControl(const StateVec & state_des, const FeedbackMat & K); /** * @brief send linear feedback control command @@ -58,14 +82,14 @@ class LinearController : public WrenchController { * @param state_des desired state * @param K feedback control matrix (i.e. u = Kx) */ - void SendControl(const ff_msgs::msg::FreeFlyerState& state_des, const FeedbackMat& K); + void SendControl(const ff_msgs::msg::FreeFlyerState & state_des, const FeedbackMat & K); /** * @brief called when the first state message is received */ virtual void StateReadyCallback(); - private: +private: StateVec state_; bool state_ready_ = false; mutable std::mutex state_mtx_; @@ -75,4 +99,4 @@ class LinearController : public WrenchController { void StateCallback(const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg); }; -} // namespace ff +} // namespace ff diff --git a/ff_control/include/ff_control/ll_ctrl.hpp b/ff_control/include/ff_control/ll_ctrl.hpp index a125111..b843170 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -1,3 +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. + + #pragma once #include @@ -9,13 +32,15 @@ #include "ff_msgs/msg/wheel_vel_command.hpp" #include "ff_params/robot_params.hpp" -namespace ff { +namespace ff +{ -class LowLevelController : virtual public rclcpp::Node { - public: +class LowLevelController : virtual public rclcpp::Node +{ +public: LowLevelController(); - protected: +protected: /** * @brief robot parameters that can be accessed by sub-classes */ @@ -26,7 +51,7 @@ class LowLevelController : virtual public rclcpp::Node { * * @param duty_cycle duty cycle for each thruster (in [0, 1]) */ - void SetThrustDutyCycle(const std::array& duty_cycle); + void SetThrustDutyCycle(const std::array & duty_cycle); /** * @brief send command to set the inertial wheel velocity @@ -34,11 +59,11 @@ class LowLevelController : virtual public rclcpp::Node { * * @param velocity velocity in [m/s] */ - void SetWheelVelocity(const double& velocity); + void SetWheelVelocity(const double & velocity); - private: +private: rclcpp::Publisher::SharedPtr thruster_pub_; rclcpp::Publisher::SharedPtr wheel_pub_; }; -} // namespace ff +} // namespace ff diff --git a/ff_control/include/ff_control/wrench_ctrl.hpp b/ff_control/include/ff_control/wrench_ctrl.hpp index 49fde4e..efc442a 100644 --- a/ff_control/include/ff_control/wrench_ctrl.hpp +++ b/ff_control/include/ff_control/wrench_ctrl.hpp @@ -1,3 +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. + + #pragma once #include @@ -6,20 +29,22 @@ #include "ff_msgs/msg/wrench2_d.hpp" -namespace ff { +namespace ff +{ -class WrenchController : public LowLevelController { - public: - WrenchController(); +class WrenchController : public LowLevelController +{ +public: + WrenchController(); - protected: +protected: /** * @brief set wrench in body frame * * @param wrench_body wrench in body frame * @param use_wheel set to true to use the inertial wheel (TODO(alvin): unsupported) */ - void SetBodyWrench(const ff_msgs::msg::Wrench2D& wrench_body, bool use_wheel = false); + void SetBodyWrench(const ff_msgs::msg::Wrench2D & wrench_body, bool use_wheel = false); /** * @brief set wrench in world frame @@ -27,10 +52,10 @@ class WrenchController : public LowLevelController { * @param wrench_world wrench in world frame * @param theta rotational state of the freeflyer */ - void SetWorldWrench(const ff_msgs::msg::Wrench2D& wrench_world, double theta); + void SetWorldWrench(const ff_msgs::msg::Wrench2D & wrench_world, double theta); - private: - ff_msgs::msg::Wrench2D ClipWrench(const ff_msgs::msg::Wrench2D& wrench) const; +private: + ff_msgs::msg::Wrench2D ClipWrench(const ff_msgs::msg::Wrench2D & wrench) const; }; -} // namespace ff +} // namespace ff diff --git a/ff_control/src/keyboard_ctrl.cpp b/ff_control/src/keyboard_ctrl.cpp index 48bece4..7c8c58f 100644 --- a/ff_control/src/keyboard_ctrl.cpp +++ b/ff_control/src/keyboard_ctrl.cpp @@ -1,3 +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. + + #include #include @@ -5,10 +28,12 @@ using namespace std::chrono_literals; -namespace ff { +namespace ff +{ KeyboardController::KeyboardController() - : rclcpp::Node("keyboard_ctrl_node") { +: rclcpp::Node("keyboard_ctrl_node") +{ // setup non blocking termio struct termios new_settings; tcgetattr(fileno(stdin), &old_settings_); @@ -23,17 +48,20 @@ KeyboardController::KeyboardController() key_timer_ = this->create_wall_timer(5ms, std::bind(&KeyboardController::KeyUpdate, this)); } -KeyboardController::~KeyboardController() { +KeyboardController::~KeyboardController() +{ // restore termio settings tcsetattr(fileno(stdin), TCSANOW, &old_settings_); fcntl(fileno(stdin), F_SETFL, f_flags_); } -char KeyboardController::GetKey() { +char KeyboardController::GetKey() +{ return last_key_.load(); } -void KeyboardController::KeyUpdate() { +void KeyboardController::KeyUpdate() +{ const char key = getchar(); if (key < 0) { if (this->get_clock()->now() - last_key_time_ > 400ms) { @@ -45,4 +73,4 @@ void KeyboardController::KeyUpdate() { } } -} // namespace ff +} // namespace ff diff --git a/ff_control/src/linear_ctrl.cpp b/ff_control/src/linear_ctrl.cpp index ed8ccb2..376a299 100644 --- a/ff_control/src/linear_ctrl.cpp +++ b/ff_control/src/linear_ctrl.cpp @@ -1,3 +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. + + #include "ff_control/linear_ctrl.hpp" using namespace std::placeholders; @@ -5,9 +28,11 @@ using ff_msgs::msg::FreeFlyerState; using ff_msgs::msg::FreeFlyerStateStamped; using ff_msgs::msg::Wrench2D; -namespace ff { +namespace ff +{ -static void State2Vec(const ff_msgs::msg::FreeFlyerState& state, LinearController::StateVec* vec) { +static void State2Vec(const ff_msgs::msg::FreeFlyerState & state, LinearController::StateVec * vec) +{ if (vec) { (*vec)[0] = state.pose.x; (*vec)[1] = state.pose.y; @@ -18,7 +43,8 @@ static void State2Vec(const ff_msgs::msg::FreeFlyerState& state, LinearControlle } } -static void Vec2State(const LinearController::StateVec& vec, ff_msgs::msg::FreeFlyerState* state) { +static void Vec2State(const LinearController::StateVec & vec, ff_msgs::msg::FreeFlyerState * state) +{ if (state) { state->pose.x = vec[0]; state->pose.y = vec[1]; @@ -30,19 +56,22 @@ static void Vec2State(const LinearController::StateVec& vec, ff_msgs::msg::FreeF } LinearController::LinearController() - : rclcpp::Node("linear_ctrl_node"), - WrenchController() { +: rclcpp::Node("linear_ctrl_node"), + WrenchController() +{ state_sub_ = this->create_subscription( "gt/state", 10, std::bind(&LinearController::StateCallback, this, _1)); } -bool LinearController::StateIsReady() const { +bool LinearController::StateIsReady() const +{ std::lock_guard lock(state_mtx_); return state_ready_; } -bool LinearController::GetState(StateVec* state) const { +bool LinearController::GetState(StateVec * state) const +{ std::lock_guard lock(state_mtx_); if (state) { @@ -52,7 +81,8 @@ bool LinearController::GetState(StateVec* state) const { return state_ready_; } -bool LinearController::GetState(FreeFlyerState* state) const { +bool LinearController::GetState(FreeFlyerState * state) const +{ std::lock_guard lock(state_mtx_); if (state) { @@ -62,15 +92,16 @@ bool LinearController::GetState(FreeFlyerState* state) const { return state_ready_; } -void LinearController::SendControl(const StateVec& state_des, const FeedbackMat& K) { +void LinearController::SendControl(const StateVec & state_des, const FeedbackMat & K) +{ StateVec state_cur; if (!GetState(&state_cur)) { RCLCPP_WARN(this->get_logger(), "SendControl ignored, state not yet ready"); - return ; + return; } StateVec state_delta = state_des - state_cur; - state_delta[2] = std::remainder(state_delta[2], 2 * M_PI); // wrap angle delta to [-pi, pi] + state_delta[2] = std::remainder(state_delta[2], 2 * M_PI); // wrap angle delta to [-pi, pi] const ControlVec u = K * state_delta; Wrench2D wrench_world{}; @@ -80,7 +111,8 @@ void LinearController::SendControl(const StateVec& state_des, const FeedbackMat& SetWorldWrench(wrench_world, state_cur[2]); } -void LinearController::SendControl(const FreeFlyerState& state_des, const FeedbackMat& K) { +void LinearController::SendControl(const FreeFlyerState & state_des, const FeedbackMat & K) +{ StateVec state_des_vec; State2Vec(state_des, &state_des_vec); SendControl(state_des_vec, K); @@ -88,7 +120,8 @@ void LinearController::SendControl(const FreeFlyerState& state_des, const Feedba void LinearController::StateReadyCallback() {} -void LinearController::StateCallback(const FreeFlyerStateStamped::SharedPtr msg) { +void LinearController::StateCallback(const FreeFlyerStateStamped::SharedPtr msg) +{ bool run_callback = false; // locked session for state_ and state_ready_ access @@ -108,4 +141,4 @@ void LinearController::StateCallback(const FreeFlyerStateStamped::SharedPtr msg) } } -} // namespace ff +} // namespace ff diff --git a/ff_control/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index 6820def..3a703e6 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -1,18 +1,44 @@ +// 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. + + #include "ff_control/ll_ctrl.hpp" using ff_msgs::msg::ThrusterCommand; using ff_msgs::msg::WheelVelCommand; -namespace ff { +namespace ff +{ LowLevelController::LowLevelController() - : rclcpp::Node("ll_ctrl_node"), - p_(this) { +: rclcpp::Node("ll_ctrl_node"), + p_(this) +{ thruster_pub_ = this->create_publisher("commands/duty_cycle", 10); wheel_pub_ = this->create_publisher("commands/velocity", 10); } -void LowLevelController::SetThrustDutyCycle(const std::array& duty_cycle) { +void LowLevelController::SetThrustDutyCycle(const std::array & duty_cycle) +{ ThrusterCommand msg{}; msg.header.stamp = this->get_clock()->now(); msg.duty_cycle = duty_cycle; @@ -20,7 +46,8 @@ void LowLevelController::SetThrustDutyCycle(const std::array& duty_cy thruster_pub_->publish(msg); } -void LowLevelController::SetWheelVelocity(const double& velocity) { +void LowLevelController::SetWheelVelocity(const double & velocity) +{ WheelVelCommand msg{}; msg.header.stamp = this->get_clock()->now(); msg.velocity = velocity; @@ -28,4 +55,4 @@ void LowLevelController::SetWheelVelocity(const double& velocity) { wheel_pub_->publish(msg); } -} // namespace ff +} // namespace ff diff --git a/ff_control/src/nodes/key_teleop_node.cpp b/ff_control/src/nodes/key_teleop_node.cpp index 89d7aa7..719d8a2 100644 --- a/ff_control/src/nodes/key_teleop_node.cpp +++ b/ff_control/src/nodes/key_teleop_node.cpp @@ -1,3 +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. + + #include #include #include @@ -13,7 +36,8 @@ using namespace std::chrono_literals; -const std::string help_msg = R"( +const char help_msg[] = + R"( This keyboard teleop node controls the linear and angular velocity of the freeflyer using a simple proportional controller. There will be large delay in your control actions due to low control authority. @@ -31,12 +55,14 @@ A/D -- translate the robot in positive / negative y direction Q/E -- rotate the robot (q counter clockwise, e clockwise) )"; -class KeyboardTeleopNode : public ff::LinearController, public ff::KeyboardController { - public: +class KeyboardTeleopNode : public ff::LinearController, public ff::KeyboardController +{ +public: KeyboardTeleopNode() - : rclcpp::Node("key_teleop_node"), - ff::LinearController(), - ff::KeyboardController() { + : rclcpp::Node("key_teleop_node"), + ff::LinearController(), + ff::KeyboardController() + { // start teleop loop telop_timer_ = this->create_wall_timer(10ms, std::bind(&KeyboardTeleopNode::TeleopLoop, this)); @@ -49,44 +75,46 @@ class KeyboardTeleopNode : public ff::LinearController, public ff::KeyboardContr K_(2, 5) = gain_dt; // print help message - RCLCPP_INFO(this->get_logger(), "%s", help_msg.c_str()); + RCLCPP_INFO(this->get_logger(), "%s", help_msg); } - private: +private: rclcpp::TimerBase::SharedPtr telop_timer_; FeedbackMat K_; - void TeleopLoop() { - if (!StateIsReady()) { return; } + void TeleopLoop() + { + if (!StateIsReady()) {return;} ff_msgs::msg::FreeFlyerState state_des{}; switch (this->GetKey()) { - case 'w': - state_des.twist.vx = 0.3; - break; - case 's': - state_des.twist.vx = -0.3; - break; - case 'a': - state_des.twist.vy = 0.3; - break; - case 'd': - state_des.twist.vy = -0.3; - break; - case 'q': - state_des.twist.wz = 0.5; - break; - case 'e': - state_des.twist.wz = -0.5; - break; + case 'w': + state_des.twist.vx = 0.3; + break; + case 's': + state_des.twist.vx = -0.3; + break; + case 'a': + state_des.twist.vy = 0.3; + break; + case 'd': + state_des.twist.vy = -0.3; + break; + case 'q': + state_des.twist.wz = 0.5; + break; + case 'e': + state_des.twist.wz = -0.5; + break; } SendControl(state_des, K_); } }; -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/ff_control/src/nodes/pd_ctrl_node.cpp b/ff_control/src/nodes/pd_ctrl_node.cpp index e15828b..9b2f5d6 100644 --- a/ff_control/src/nodes/pd_ctrl_node.cpp +++ b/ff_control/src/nodes/pd_ctrl_node.cpp @@ -1,3 +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. + + #include #include #include @@ -15,13 +38,15 @@ using namespace std::chrono_literals; using namespace std::placeholders; -class PDControlNode : public ff::LinearController { - public: +class PDControlNode : public ff::LinearController +{ +public: PDControlNode() - : rclcpp::Node("pd_control_node"), - ff::LinearController(), - state_des_{}, - K_(FeedbackMat::Zero()) { + : rclcpp::Node("pd_control_node"), + ff::LinearController(), + state_des_{}, + K_(FeedbackMat::Zero()) + { state_setpoint_sub_ = this->create_subscription( "ctrl/state", 10, std::bind(&PDControlNode::SetpointCallback, this, _1)); rviz_setpoint_sub_ = this->create_subscription( @@ -42,7 +67,7 @@ class PDControlNode : public ff::LinearController { K_(2, 5) = gain_dt; } - private: +private: rclcpp::Subscription::SharedPtr state_setpoint_sub_; rclcpp::Subscription::SharedPtr rviz_setpoint_sub_; rclcpp::TimerBase::SharedPtr timer_; @@ -51,24 +76,28 @@ class PDControlNode : public ff::LinearController { FeedbackMat K_; - void StateReadyCallback() override { + void StateReadyCallback() override + { // copy current position as goal position state_des_.header.stamp = this->get_clock()->now(); GetState(&state_des_.state); } - void ControlLoop() { + void ControlLoop() + { // state not yet ready - if (!StateIsReady()) { return; } + if (!StateIsReady()) {return;} SendControl(state_des_.state, K_); } - void SetpointCallback(const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg) { + void SetpointCallback(const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg) + { state_des_ = *msg; } - void GoalPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + void GoalPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { state_des_.state.pose.x = msg->pose.position.x; state_des_.state.pose.y = msg->pose.position.y; @@ -82,7 +111,8 @@ class PDControlNode : public ff::LinearController { } }; -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/ff_control/src/nodes/wrench_ctrl_node.cpp b/ff_control/src/nodes/wrench_ctrl_node.cpp index 474d59b..af9ff1f 100644 --- a/ff_control/src/nodes/wrench_ctrl_node.cpp +++ b/ff_control/src/nodes/wrench_ctrl_node.cpp @@ -1,3 +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. + + #include #include #include @@ -11,11 +34,13 @@ using namespace std::placeholders; -class WrenchControlNode : public ff::WrenchController { - public: +class WrenchControlNode : public ff::WrenchController +{ +public: WrenchControlNode() - : rclcpp::Node("wrench_control_node"), - ff::WrenchController() { + : rclcpp::Node("wrench_control_node"), + ff::WrenchController() + { wrench_body_sub_ = this->create_subscription( "ctrl/wrench_body", 10, std::bind(&WrenchControlNode::WrenchBodyCallback, this, _1)); wrench_world_sub_ = this->create_subscription( @@ -24,27 +49,31 @@ class WrenchControlNode : public ff::WrenchController { "gt/pose", 10, std::bind(&WrenchControlNode::PoseCallback, this, _1)); } - private: +private: rclcpp::Subscription::SharedPtr wrench_body_sub_; rclcpp::Subscription::SharedPtr wrench_world_sub_; rclcpp::Subscription::SharedPtr pose_sub_; ff_msgs::msg::Pose2DStamped pose_; - void WrenchBodyCallback(const ff_msgs::msg::Wrench2D::SharedPtr msg) { + void WrenchBodyCallback(const ff_msgs::msg::Wrench2D::SharedPtr msg) + { SetBodyWrench(*msg); } - void WrenchWorldCallback(const ff_msgs::msg::Wrench2D::SharedPtr msg) { + void WrenchWorldCallback(const ff_msgs::msg::Wrench2D::SharedPtr msg) + { SetWorldWrench(*msg, pose_.pose.theta); } - void PoseCallback(const ff_msgs::msg::Pose2DStamped::SharedPtr msg) { + void PoseCallback(const ff_msgs::msg::Pose2DStamped::SharedPtr msg) + { pose_ = *msg; } }; -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/ff_control/src/wrench_ctrl.cpp b/ff_control/src/wrench_ctrl.cpp index c896823..2ae1d2e 100644 --- a/ff_control/src/wrench_ctrl.cpp +++ b/ff_control/src/wrench_ctrl.cpp @@ -1,16 +1,41 @@ +// 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. + + #include #include "ff_control/wrench_ctrl.hpp" using ff_msgs::msg::Wrench2D; -namespace ff { +namespace ff +{ WrenchController::WrenchController() - : rclcpp::Node("wrench_ctrl_node"), - LowLevelController() {} +: rclcpp::Node("wrench_ctrl_node"), + LowLevelController() {} -void WrenchController::SetBodyWrench(const Wrench2D& wrench_body, bool use_wheel) { +void WrenchController::SetBodyWrench(const Wrench2D & wrench_body, bool use_wheel) +{ if (use_wheel) { // TODO(alvin): support wheel velocity control in the future or remove? RCLCPP_ERROR(this->get_logger(), "SetWrench failed: use_wheel not implemented"); @@ -42,11 +67,11 @@ void WrenchController::SetBodyWrench(const Wrench2D& wrench_body, bool use_wheel const double u_M = wrench_body_clipped.tz / (4 * p_.actuators.F_max_per_thruster * p_.actuators.thrusters_lever_arm); if (u_M > 0) { - for (const int& i : {1, 3, 5, 7}) { + for (const int & i : {1, 3, 5, 7}) { duty_cycle[i] += u_M; } } else { - for (const int& i : {0, 2, 4, 6}) { + for (const int & i : {0, 2, 4, 6}) { duty_cycle[i] += -u_M; } } @@ -60,7 +85,8 @@ void WrenchController::SetBodyWrench(const Wrench2D& wrench_body, bool use_wheel } } -void WrenchController::SetWorldWrench(const ff_msgs::msg::Wrench2D& wrench_world, double theta) { +void WrenchController::SetWorldWrench(const ff_msgs::msg::Wrench2D & wrench_world, double theta) +{ const double cos_theta = std::cos(theta); const double sin_theta = std::sin(theta); @@ -71,7 +97,8 @@ void WrenchController::SetWorldWrench(const ff_msgs::msg::Wrench2D& wrench_world SetBodyWrench(wrench_body); } -Wrench2D WrenchController::ClipWrench(const Wrench2D& wrench) const { +Wrench2D WrenchController::ClipWrench(const Wrench2D & wrench) const +{ Wrench2D wrench_clipped; const double force = std::sqrt(wrench.fx * wrench.fx + wrench.fy * wrench.fy); const double force_scale = std::min(p_.actuators.F_body_max / force, 1.0); @@ -84,4 +111,4 @@ Wrench2D WrenchController::ClipWrench(const Wrench2D& wrench) const { return wrench_clipped; } -} // namespace ff +} // namespace ff From bc0cb6c254e733370f39075bc46f957f836257ee Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 13:43:09 -0700 Subject: [PATCH 29/56] remove style test for ff_sim --- ff_sim/ff_sim/simulator_node.py | 54 +++++++++++++++++++++++++++++---- ff_sim/launch/single.launch.py | 23 ++++++++++++++ ff_sim/test/test_copyright.py | 1 - ff_sim/test/test_flake8.py | 25 --------------- ff_sim/test/test_pep257.py | 23 -------------- 5 files changed, 71 insertions(+), 55 deletions(-) delete mode 100644 ff_sim/test/test_flake8.py delete mode 100644 ff_sim/test/test_pep257.py diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 99bfd40..f61402b 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -1,8 +1,33 @@ -""" ----------------------------------------------- - Simulates the freeflyer - Maps thrusters duty cycle + wheel control input - to a state evolution over time ------------------------------------------------ """ +# 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. + + +""" +Simulates the freeflyer. + +Maps thrusters duty cycle + wheel control input +to a state evolution over time +----------------------------------------------- +""" import sys import numpy as np @@ -20,6 +45,22 @@ def simulate_contact( robot_radius: float, obstacles: T.Dict[str, T.Any] ) -> T.Tuple[np.ndarray, np.ndarray, bool]: + """ + Simulate contact with obstacles. + + Args: + state (np.ndarray): robot state in [x, y, theta, vx, vy, omega] + robot_radius (float): robot radius + obstacles (T.Dict[str, T.Any]): cylinder obstacle attributes including + cyl_pos_x -- x coordinates + cyl_pos_y -- y coordinates + cyl_rads -- radii + cyl_heights -- cylinder heights (for visualization) + + Returns: + T.Tuple[np.ndarray, np.ndarray, bool]: updated state, contact wrench, contact flag + + """ B_contact = False F_MAG_NOM = 6. # in [N] @@ -56,7 +97,8 @@ def simulate_contact( return state, W_contact, B_contact class FreeFlyerSimulator(Node): - """Free-Flyer simulator class""" + """Free-Flyer simulator class.""" + def __init__(self): super().__init__("ff_sim_node") self.x_dim = 6 # state dimension diff --git a/ff_sim/launch/single.launch.py b/ff_sim/launch/single.launch.py index d6d8365..53edc62 100644 --- a/ff_sim/launch/single.launch.py +++ b/ff_sim/launch/single.launch.py @@ -1,3 +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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/ff_sim/test/test_copyright.py b/ff_sim/test/test_copyright.py index 97a3919..590189a 100644 --- a/ff_sim/test/test_copyright.py +++ b/ff_sim/test/test_copyright.py @@ -17,7 +17,6 @@ # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') @pytest.mark.copyright @pytest.mark.linter def test_copyright(): diff --git a/ff_sim/test/test_flake8.py b/ff_sim/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/ff_sim/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 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_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/ff_sim/test/test_pep257.py b/ff_sim/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/ff_sim/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' From 833f6619fd034641e9ec67ab34a3057627c00535 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 13:47:09 -0700 Subject: [PATCH 30/56] fix styles for ff_params --- ff_params/CMakeLists.txt | 7 -- ff_params/ff_params/__init__.py | 24 +++++++ ff_params/include/ff_params/robot_params.hpp | 41 +++++++++-- ff_params/src/robot_params.cpp | 72 ++++++++++++++------ ff_params/src/robot_params_node.cpp | 46 ++++++++++--- 5 files changed, 146 insertions(+), 44 deletions(-) diff --git a/ff_params/CMakeLists.txt b/ff_params/CMakeLists.txt index 5555e7b..8271048 100644 --- a/ff_params/CMakeLists.txt +++ b/ff_params/CMakeLists.txt @@ -45,13 +45,6 @@ ament_python_install_package(${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_params/ff_params/__init__.py b/ff_params/ff_params/__init__.py index 989eb75..0d931bb 100644 --- a/ff_params/ff_params/__init__.py +++ b/ff_params/ff_params/__init__.py @@ -1,3 +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. + + import typing as T import numpy as np @@ -6,6 +29,7 @@ from rclpy.node import Node from rcl_interfaces.srv import GetParameters + class RobotParams: def __init__(self, node: Node, diff --git a/ff_params/include/ff_params/robot_params.hpp b/ff_params/include/ff_params/robot_params.hpp index 81ccce6..f16b742 100644 --- a/ff_params/include/ff_params/robot_params.hpp +++ b/ff_params/include/ff_params/robot_params.hpp @@ -1,3 +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. + + #pragma once #include @@ -7,9 +30,11 @@ #include -namespace ff { +namespace ff +{ -struct DynamicsParams { +struct DynamicsParams +{ double mass; double inertia; double radius; @@ -17,7 +42,8 @@ struct DynamicsParams { std::vector force_const; }; -struct ActuatorParams { +struct ActuatorParams +{ double F_max_per_thruster; double thrusters_lever_arm; double F_body_max; @@ -28,16 +54,17 @@ struct ActuatorParams { double gamma_max; }; -class RobotParams { - public: +class RobotParams +{ +public: DynamicsParams dynamics; ActuatorParams actuators; - RobotParams(rclcpp::Node* node); + RobotParams(rclcpp::Node * node); bool Loaded() const; - private: +private: std::atomic loaded_ = false; rclcpp::AsyncParametersClient::SharedPtr param_client_; diff --git a/ff_params/src/robot_params.cpp b/ff_params/src/robot_params.cpp index 6eff649..1c9f5d8 100644 --- a/ff_params/src/robot_params.cpp +++ b/ff_params/src/robot_params.cpp @@ -1,3 +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. + + #include "ff_params/robot_params.hpp" #include @@ -5,39 +28,44 @@ using namespace std::chrono_literals; using namespace std::placeholders; -namespace ff { +namespace ff +{ -RobotParams::RobotParams(rclcpp::Node* node) - : loaded_(false), - param_client_(std::make_shared( - node, std::string(node->get_namespace()) + "/robot_params_node")) { +RobotParams::RobotParams(rclcpp::Node * node) +: loaded_(false), + param_client_(std::make_shared( + node, std::string(node->get_namespace()) + "/robot_params_node")) +{ while (!param_client_->service_is_ready()) { param_client_->wait_for_service(5s); RCLCPP_INFO(node->get_logger(), "parameter service not ready, retrying..."); } - param_client_->get_parameters({ - "dynamics.mass", - "dynamics.inertia", - "dynamics.radius", - "dynamics.CoM_offset", - "dynamics.force_const", - "actuators.F_max_per_thruster", - "actuators.thrusters_lever_arm", - "actuators.F_body_max", - "actuators.M_body_max", - "actuators.min_inp_percent", - "actuators.max_inp_percent", - "actuators.gamma_min", - "actuators.gamma_max", - }, std::bind(&RobotParams::ParamReadyCallback, this, _1)); + param_client_->get_parameters( + { + "dynamics.mass", + "dynamics.inertia", + "dynamics.radius", + "dynamics.CoM_offset", + "dynamics.force_const", + "actuators.F_max_per_thruster", + "actuators.thrusters_lever_arm", + "actuators.F_body_max", + "actuators.M_body_max", + "actuators.min_inp_percent", + "actuators.max_inp_percent", + "actuators.gamma_min", + "actuators.gamma_max", + }, std::bind(&RobotParams::ParamReadyCallback, this, _1)); } -bool RobotParams::Loaded() const { +bool RobotParams::Loaded() const +{ return loaded_.load(); } -void RobotParams::ParamReadyCallback(std::shared_future> future) { +void RobotParams::ParamReadyCallback(std::shared_future> future) +{ const auto params = future.get(); this->dynamics.mass = params[0].as_double(); diff --git a/ff_params/src/robot_params_node.cpp b/ff_params/src/robot_params_node.cpp index afeb841..c63503a 100644 --- a/ff_params/src/robot_params_node.cpp +++ b/ff_params/src/robot_params_node.cpp @@ -1,3 +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. + + #include #include #include @@ -6,22 +29,28 @@ using namespace std::chrono_literals; -class RobotParamsNode : public rclcpp::Node { - public: - explicit RobotParamsNode(const std::string& node_name) : rclcpp::Node(node_name) { +class RobotParamsNode : public rclcpp::Node +{ +public: + explicit RobotParamsNode(const std::string & node_name) + : rclcpp::Node(node_name) + { // dynamics - this->declare_parameters("dynamics", { + this->declare_parameters( + "dynamics", { {"mass", 16.}, // mass [kg] {"inertia", 0.18}, // inertia at CoM (no payload) [kg*m^2] {"radius", 0.1}, // robot radius, in [m] }); - this->declare_parameters>("dynamics", { + this->declare_parameters>( + "dynamics", { {"CoM_offset", {0., 0.}}, // center of mass, in body frame [m] {"force_const", {0.005, 0.005}}, // constant force (e.g. table tilt), in world frame [N] }); // actuators - this->declare_parameters("actuators", { + this->declare_parameters( + "actuators", { {"F_max_per_thruster", 0.2}, // max force per thruster, in [N] {"thrusters_lever_arm", 0.11461}, // lever arm, in [m] {"F_body_max", 0.4}, // body frame force max, in [N] @@ -35,11 +64,12 @@ class RobotParamsNode : public rclcpp::Node { timer_ = this->create_wall_timer(1h, []() {}); } - private: +private: rclcpp::TimerBase::SharedPtr timer_; }; -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared("robot_params_node")); rclcpp::shutdown(); From edce087e514ed2b22110d3d318947322d8597015 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 13:50:33 -0700 Subject: [PATCH 31/56] fix tests for ff_msgs --- ff_msgs/CMakeLists.txt | 7 ------- ff_msgs/package.xml | 4 ++-- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/ff_msgs/CMakeLists.txt b/ff_msgs/CMakeLists.txt index 37255b1..0ef3928 100644 --- a/ff_msgs/CMakeLists.txt +++ b/ff_msgs/CMakeLists.txt @@ -26,13 +26,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_msgs/package.xml b/ff_msgs/package.xml index b2ecaad..108ea24 100644 --- a/ff_msgs/package.xml +++ b/ff_msgs/package.xml @@ -15,11 +15,11 @@ rosidl_default_runtime - rosidl_interface_packages - ament_lint_auto ament_lint_common + rosidl_interface_packages + ament_cmake From b3739b2c398b54ff45adb0023d0bdcd2c994b986 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 13:54:07 -0700 Subject: [PATCH 32/56] fix tests for ff_viz --- ff_viz/CMakeLists.txt | 7 ------ ff_viz/launch/ff_viz.launch.py | 24 ++++++++++++++++++ ff_viz/src/renderer_node.cpp | 45 ++++++++++++++++++++++++++++------ 3 files changed, 61 insertions(+), 15 deletions(-) diff --git a/ff_viz/CMakeLists.txt b/ff_viz/CMakeLists.txt index 1ff0e65..604efb8 100644 --- a/ff_viz/CMakeLists.txt +++ b/ff_viz/CMakeLists.txt @@ -34,13 +34,6 @@ install(TARGETS renderer_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_viz/launch/ff_viz.launch.py b/ff_viz/launch/ff_viz.launch.py index 0d42464..dd6b052 100644 --- a/ff_viz/launch/ff_viz.launch.py +++ b/ff_viz/launch/ff_viz.launch.py @@ -1,3 +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. + + import os from ament_index_python.packages import get_package_share_directory @@ -7,6 +30,7 @@ from launch_ros.actions import Node from launch_ros.descriptions import ParameterValue + def generate_launch_description(): urdf = os.path.join(get_package_share_directory("ff_viz"), "model", "ff.urdf.xacro") robot_name = LaunchConfiguration("robot_name") diff --git a/ff_viz/src/renderer_node.cpp b/ff_viz/src/renderer_node.cpp index 9e883a4..7e72854 100644 --- a/ff_viz/src/renderer_node.cpp +++ b/ff_viz/src/renderer_node.cpp @@ -1,3 +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. + + #include #include #include @@ -15,9 +38,12 @@ using namespace std::chrono_literals; using namespace std::placeholders; -class Renderer : public rclcpp::Node { - public: - explicit Renderer(const std::string& name) : rclcpp::Node(name) { +class Renderer : public rclcpp::Node +{ +public: + explicit Renderer(const std::string & name) + : rclcpp::Node(name) + { auto robot_names = this->declare_parameter>("robot_name", {"robot"}); state_subs_.resize(robot_names.size()); transform_msgs_.resize(robot_names.size()); @@ -62,7 +88,7 @@ class Renderer : public rclcpp::Node { state_subs_[i] = this->create_subscription( "/" + robot_names[i] + "/gt/state", 10, - [this, i](const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg){ + [this, i](const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg) { this->StateCallback(i, msg); } ); @@ -78,7 +104,7 @@ class Renderer : public rclcpp::Node { timer_ = this->create_wall_timer(20ms, std::bind(&Renderer::VizUpdate, this)); } - private: +private: rclcpp::TimerBase::SharedPtr timer_; std::vector::SharedPtr> state_subs_; @@ -88,7 +114,8 @@ class Renderer : public rclcpp::Node { std::unique_ptr tf_broadcaster_; std::vector transform_msgs_; - void VizUpdate() { + void VizUpdate() + { // render table marker_msg_.header.stamp = this->get_clock()->now(); marker_pub_->publish(marker_msg_); @@ -99,7 +126,8 @@ class Renderer : public rclcpp::Node { } } - void StateCallback(const size_t idx, const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg) { + void StateCallback(const size_t idx, const ff_msgs::msg::FreeFlyerStateStamped::SharedPtr msg) + { transform_msgs_[idx]->transform.translation.x = msg->state.pose.x; transform_msgs_[idx]->transform.translation.y = msg->state.pose.y; transform_msgs_[idx]->transform.translation.z = 0.15; @@ -112,7 +140,8 @@ class Renderer : public rclcpp::Node { } }; -int main(int argc, char ** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared("renderer_node")); rclcpp::shutdown(); From 025c17b6a5f6c724230a2e13453b46058cf25b48 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 14:00:39 -0700 Subject: [PATCH 33/56] fix cpplint --- ff_params/include/ff_params/robot_params.hpp | 4 ++-- ff_params/src/robot_params.cpp | 2 +- ff_params/src/robot_params_node.cpp | 10 +++++----- ff_viz/src/renderer_node.cpp | 3 ++- freeflyer/CMakeLists.txt | 7 ------- freeflyer/launch/sim_key_teleop.launch.py | 1 + freeflyer/launch/sim_pd.launch.py | 1 + 7 files changed, 12 insertions(+), 16 deletions(-) diff --git a/ff_params/include/ff_params/robot_params.hpp b/ff_params/include/ff_params/robot_params.hpp index f16b742..f4462cb 100644 --- a/ff_params/include/ff_params/robot_params.hpp +++ b/ff_params/include/ff_params/robot_params.hpp @@ -60,7 +60,7 @@ class RobotParams DynamicsParams dynamics; ActuatorParams actuators; - RobotParams(rclcpp::Node * node); + explicit RobotParams(rclcpp::Node * node); bool Loaded() const; @@ -71,4 +71,4 @@ class RobotParams void ParamReadyCallback(std::shared_future> future); }; -} // namespace ff +} // namespace ff diff --git a/ff_params/src/robot_params.cpp b/ff_params/src/robot_params.cpp index 1c9f5d8..f960932 100644 --- a/ff_params/src/robot_params.cpp +++ b/ff_params/src/robot_params.cpp @@ -86,4 +86,4 @@ void RobotParams::ParamReadyCallback(std::shared_futuredeclare_parameters( "dynamics", { - {"mass", 16.}, // mass [kg] - {"inertia", 0.18}, // inertia at CoM (no payload) [kg*m^2] - {"radius", 0.1}, // robot radius, in [m] + {"mass", 16.}, // mass [kg] + {"inertia", 0.18}, // inertia at CoM (no payload) [kg*m^2] + {"radius", 0.1}, // robot radius, in [m] }); this->declare_parameters>( "dynamics", { @@ -51,8 +51,8 @@ class RobotParamsNode : public rclcpp::Node // actuators this->declare_parameters( "actuators", { - {"F_max_per_thruster", 0.2}, // max force per thruster, in [N] - {"thrusters_lever_arm", 0.11461}, // lever arm, in [m] + {"F_max_per_thruster", 0.2}, // max force per thruster, in [N] + {"thrusters_lever_arm", 0.11461}, // lever arm, in [m] {"F_body_max", 0.4}, // body frame force max, in [N] {"M_body_max", 0.05}, // body frame moment / torque max, in [Nm] {"min_inp_percent", 0.05}, // in [0,0.15] - inputs are zero close to min input diff --git a/ff_viz/src/renderer_node.cpp b/ff_viz/src/renderer_node.cpp index 7e72854..0593dc1 100644 --- a/ff_viz/src/renderer_node.cpp +++ b/ff_viz/src/renderer_node.cpp @@ -21,6 +21,8 @@ // SOFTWARE. +#include + #include #include #include @@ -30,7 +32,6 @@ #include #include -#include #include #include "ff_msgs/msg/free_flyer_state_stamped.hpp" diff --git a/freeflyer/CMakeLists.txt b/freeflyer/CMakeLists.txt index 950a6b6..36df4eb 100644 --- a/freeflyer/CMakeLists.txt +++ b/freeflyer/CMakeLists.txt @@ -17,13 +17,6 @@ install(DIRECTORY launch if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index 638242e..ddb79aa 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -4,6 +4,7 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 4fcb5ad..7863734 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -4,6 +4,7 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") From 890b20132e99c0cd5bea3b8535d69da81b40bec1 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 14:01:38 -0700 Subject: [PATCH 34/56] add copyright to freeflyer --- freeflyer/launch/sim_key_teleop.launch.py | 23 +++++++++++++++++++++++ freeflyer/launch/sim_pd.launch.py | 23 +++++++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index ddb79aa..682ab21 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -1,3 +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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.substitutions import LaunchConfiguration, PathJoinSubstitution diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 7863734..7a11bde 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -1,3 +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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.substitutions import LaunchConfiguration, PathJoinSubstitution From 6aa580d00a364b91fc8019708c3b73b74690e51e Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 14:02:21 -0700 Subject: [PATCH 35/56] include test to ROS2 CI --- .github/workflows/humble.yml | 1 - .github/workflows/rolling.yml | 1 - 2 files changed, 2 deletions(-) diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index c37f7ce..6a19c3b 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -23,4 +23,3 @@ jobs: with: import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: humble - skip-tests: true diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 8da13a4..7b45ce2 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -23,4 +23,3 @@ jobs: with: import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: rolling - skip-tests: true From 17070d731f908ec512cf702ed1434ad8daad38e6 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 15:36:08 -0700 Subject: [PATCH 36/56] fix some python tests --- README.md | 42 +++++++++++++++ ff_params/ff_params/__init__.py | 66 +++++++++++------------ ff_viz/launch/ff_viz.launch.py | 34 ++++++------ freeflyer/launch/sim_key_teleop.launch.py | 28 +++++----- freeflyer/launch/sim_pd.launch.py | 26 ++++----- 5 files changed, 118 insertions(+), 78 deletions(-) diff --git a/README.md b/README.md index 553246a..c5cde51 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,48 @@ cd ~/ff_ws && colcon build source ~/ff_ws/install/local_setup.bash ``` +## Development Guide +The CI will build and run all registered tests. PR can be merged only when all status checks +pass. [ROS2 code styles](https://docs.ros.org/en/humble/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html) +are enforced for both C++ and Python. Here are some tips for fixing code style issues for a PR. + +#### Copyright +Every source file (e.g. `.cpp`, `.hpp`, `.py`) including launch files should have a copy of +the license at the very top. See any source files for an example. + +#### C++ Code Style +1. Run `ament_uncrustify --reformat` to automatically format C++ source files. +2. Run `ament_cpplint` to check for style violations and fix them manually. + +#### Python Code Style +For the first time, you will need to run the following command to install some dev packages +```sh +sudo apt install -y \ + python3-flake8-docstrings \ + python3-flake8-blind-except \ + python3-flake8-builtins \ + python3-flake8-class-newline \ + python3-flake8-comprehensions \ + python3-flake8-deprecated \ + python3-flake8-import-order \ + python3-flake8-quotes \ + python3-pytest-repeat \ + python3-pytest-rerunfailures +``` +1. Run `ament_flake8` to check for code style violations and fix them manually. +2. Run `ament_pep257` to check for docstring style violations and fix them manually. + +#### Disable Code Style Test +If you hate the ROS2 conventions so bad, you can disable specific code style test by adding +the following line before `ament_lint_auto_find_test_dependencies()` in `CMakeLists.txt`. +```cmake +set(AMENT_LINT_AUTO_EXCLUDE [ ...]) +``` +For example, if you want to disable code style checks for all Python files, you can add +```cmake +set(AMENT_LINT_AUTO_EXCLUDE ament_cmake_flake8 ament_cmake_pep257) +``` + ## Package Layout * `freeflyer` -- top level pacakge (contains just launch files) diff --git a/ff_params/ff_params/__init__.py b/ff_params/ff_params/__init__.py index 0d931bb..b358af4 100644 --- a/ff_params/ff_params/__init__.py +++ b/ff_params/ff_params/__init__.py @@ -22,42 +22,40 @@ import typing as T -import numpy as np - -import rclpy -from rclpy.node import Node +import numpy as np from rcl_interfaces.srv import GetParameters +import rclpy class RobotParams: def __init__(self, - node: Node, + node: rclpy.Node, param_ready_cb: T.Optional[T.Callable[[], None]] = None): robot_name = node.get_namespace() - cli = node.create_client(GetParameters, f"{robot_name}/robot_params_node/get_parameters") + cli = node.create_client(GetParameters, f'{robot_name}/robot_params_node/get_parameters') self.node = node self.param_ready_cb = param_ready_cb while not cli.service_is_ready(): cli.wait_for_service(5.) - node.get_logger().info("parameter service not ready, retrying...") + node.get_logger().info('parameter service not ready, retrying...') self.request = GetParameters.Request() self.request.names = [ - "dynamics.mass", - "dynamics.inertia", - "dynamics.radius", - "dynamics.CoM_offset", - "dynamics.force_const", - "actuators.F_max_per_thruster", - "actuators.thrusters_lever_arm", - "actuators.F_body_max", - "actuators.M_body_max", - "actuators.min_inp_percent", - "actuators.max_inp_percent", - "actuators.gamma_min", - "actuators.gamma_max", + 'dynamics.mass', + 'dynamics.inertia', + 'dynamics.radius', + 'dynamics.CoM_offset', + 'dynamics.force_const', + 'actuators.F_max_per_thruster', + 'actuators.thrusters_lever_arm', + 'actuators.F_body_max', + 'actuators.M_body_max', + 'actuators.min_inp_percent', + 'actuators.max_inp_percent', + 'actuators.gamma_min', + 'actuators.gamma_max', ] future = cli.call_async(self.request) @@ -69,25 +67,25 @@ def _param_ready_cb(self, future: rclpy.task.Future): try: res = future.result() except Exception as e: - self.node.get_logger().error("get robot params failed: %r" % (e,)) + self.node.get_logger().error('get robot params failed: %r' % (e,)) else: self.dynamics = { - "mass": res.values[0].double_value, - "inertia": res.values[1].double_value, - "radius": res.values[2].double_value, - "CoM_offset": np.array(res.values[3].double_array_value), - "force_const": np.array(res.values[4].double_array_value), + 'mass': res.values[0].double_value, + 'inertia': res.values[1].double_value, + 'radius': res.values[2].double_value, + 'CoM_offset': np.array(res.values[3].double_array_value), + 'force_const': np.array(res.values[4].double_array_value), } self.actuators = { - "F_max_per_thruster": res.values[5].double_value, - "thrusters_lever_arm": res.values[6].double_value, - "F_body_max": res.values[7].double_value, - "M_body_max": res.values[8].double_value, - "min_inp_percent": res.values[9].double_value, - "max_inp_percent": res.values[10].double_value, - "gamma_min": res.values[11].double_value, - "gamma_max": res.values[12].double_value, + 'F_max_per_thruster': res.values[5].double_value, + 'thrusters_lever_arm': res.values[6].double_value, + 'F_body_max': res.values[7].double_value, + 'M_body_max': res.values[8].double_value, + 'min_inp_percent': res.values[9].double_value, + 'max_inp_percent': res.values[10].double_value, + 'gamma_min': res.values[11].double_value, + 'gamma_max': res.values[12].double_value, } self.loaded = True diff --git a/ff_viz/launch/ff_viz.launch.py b/ff_viz/launch/ff_viz.launch.py index dd6b052..8d50ec3 100644 --- a/ff_viz/launch/ff_viz.launch.py +++ b/ff_viz/launch/ff_viz.launch.py @@ -32,37 +32,37 @@ def generate_launch_description(): - urdf = os.path.join(get_package_share_directory("ff_viz"), "model", "ff.urdf.xacro") - robot_name = LaunchConfiguration("robot_name") + urdf = os.path.join(get_package_share_directory('ff_viz'), 'model', 'ff.urdf.xacro') + robot_name = LaunchConfiguration('robot_name') return LaunchDescription([ - DeclareLaunchArgument("robot_name", default_value="robot"), + DeclareLaunchArgument('robot_name', default_value='robot'), Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', namespace=robot_name, parameters=[{ - "robot_description": ParameterValue( - Command(["xacro ", urdf, " robot_name:=", robot_name]), + 'robot_description': ParameterValue( + Command(['xacro ', urdf, ' robot_name:=', robot_name]), value_type=str ) }], ), Node( - package="ff_viz", - executable="renderer_node", - name="renderer_node", - parameters=[{"robot_names": [robot_name]}] + package='ff_viz', + executable='renderer_node', + name='renderer_node', + parameters=[{'robot_names': [robot_name]}] ), Node( - package="rviz2", - executable="rviz2", - name="rviz2", + package='rviz2', + executable='rviz2', + name='rviz2', namespace=robot_name, arguments=[ - "-d", - os.path.join(get_package_share_directory("ff_viz"), "rviz", "default.rviz"), + '-d', + os.path.join(get_package_share_directory('ff_viz'), 'rviz', 'default.rviz'), ], ) ]) diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index 682ab21..3d90ed9 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -29,31 +29,31 @@ def generate_launch_description(): - robot_name = LaunchConfiguration("robot_name") + robot_name = LaunchConfiguration('robot_name') return LaunchDescription([ - DeclareLaunchArgument("robot_name", default_value="robot"), + DeclareLaunchArgument('robot_name', default_value='robot'), IncludeLaunchDescription( PathJoinSubstitution([ - FindPackageShare("ff_sim"), - "launch", - "single.launch.py", + FindPackageShare('ff_sim'), + 'launch', + 'single.launch.py', ]), - launch_arguments={"robot_name": robot_name}.items(), + launch_arguments={'robot_name': robot_name}.items(), ), IncludeLaunchDescription( PathJoinSubstitution([ - FindPackageShare("ff_viz"), - "launch", - "ff_viz.launch.py", + FindPackageShare('ff_viz'), + 'launch', + 'ff_viz.launch.py', ]), - launch_arguments={"robot_name": robot_name}.items(), + launch_arguments={'robot_name': robot_name}.items(), ), Node( - package="ff_control", - executable="key_teleop_node", - name="key_teleop_node", + package='ff_control', + executable='key_teleop_node', + name='key_teleop_node', namespace=robot_name, - prefix="gnome-terminal --", + prefix='gnome-terminal --', ), ]) diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 7a11bde..5da4444 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -29,30 +29,30 @@ def generate_launch_description(): - robot_name = LaunchConfiguration("robot_name") + robot_name = LaunchConfiguration('robot_name') return LaunchDescription([ - DeclareLaunchArgument("robot_name", default_value="robot"), + DeclareLaunchArgument('robot_name', default_value='robot'), IncludeLaunchDescription( PathJoinSubstitution([ - FindPackageShare("ff_sim"), - "launch", - "single.launch.py", + FindPackageShare('ff_sim'), + 'launch', + 'single.launch.py', ]), - launch_arguments={"robot_name": robot_name}.items(), + launch_arguments={'robot_name': robot_name}.items(), ), IncludeLaunchDescription( PathJoinSubstitution([ - FindPackageShare("ff_viz"), - "launch", - "ff_viz.launch.py", + FindPackageShare('ff_viz'), + 'launch', + 'ff_viz.launch.py', ]), - launch_arguments={"robot_name": robot_name}.items(), + launch_arguments={'robot_name': robot_name}.items(), ), Node( - package="ff_control", - executable="pd_ctrl_node", - name="pd_ctrl_node", + package='ff_control', + executable='pd_ctrl_node', + name='pd_ctrl_node', namespace=robot_name, ), ]) From a582536e11285e2d1e004fde784ea1885586dc85 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 15:47:46 -0700 Subject: [PATCH 37/56] fix typo --- ff_params/ff_params/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ff_params/ff_params/__init__.py b/ff_params/ff_params/__init__.py index b358af4..a14dfcb 100644 --- a/ff_params/ff_params/__init__.py +++ b/ff_params/ff_params/__init__.py @@ -30,7 +30,7 @@ class RobotParams: def __init__(self, - node: rclpy.Node, + node: rclpy.node.Node, param_ready_cb: T.Optional[T.Callable[[], None]] = None): robot_name = node.get_namespace() cli = node.create_client(GetParameters, f'{robot_name}/robot_params_node/get_parameters') From 0bcbe943488bfbd3d41477b6ccaeabecb5fac4bb Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 20:21:17 -0700 Subject: [PATCH 38/56] add pd ctrl node test --- freeflyer/CMakeLists.txt | 3 + freeflyer/package.xml | 2 + freeflyer/test/pd_ctrl_launch_test.py | 133 ++++++++++++++++++++++++++ 3 files changed, 138 insertions(+) create mode 100644 freeflyer/test/pd_ctrl_launch_test.py diff --git a/freeflyer/CMakeLists.txt b/freeflyer/CMakeLists.txt index 36df4eb..a55d074 100644 --- a/freeflyer/CMakeLists.txt +++ b/freeflyer/CMakeLists.txt @@ -17,7 +17,10 @@ install(DIRECTORY launch if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + ament_lint_auto_find_test_dependencies() + add_launch_test(test/pd_ctrl_launch_test.py) endif() ament_package() diff --git a/freeflyer/package.xml b/freeflyer/package.xml index 42d347c..5139303 100644 --- a/freeflyer/package.xml +++ b/freeflyer/package.xml @@ -17,6 +17,8 @@ ament_lint_auto ament_lint_common + ff_msgs + launch_testing_ament_cmake ament_cmake diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py new file mode 100644 index 0000000..6f4ac58 --- /dev/null +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -0,0 +1,133 @@ +# 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 unittest + +from ff_msgs.msg import FreeFlyerStateStamped + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest + +import rclpy + + +ROBOT_NAME = 'freeflyer' + + +def generate_test_description(): + sim_launch = IncludeLaunchDescription( + PathJoinSubstitution([ + FindPackageShare('ff_sim'), + 'launch', + 'single.launch.py', + ]), + launch_arguments={'robot_name': ROBOT_NAME}.items(), + ) + pd_ctrl_node = Node( + package='ff_control', + executable='pd_ctrl_node', + name='pd_ctrl_node', + namespace=ROBOT_NAME, + ) + + return LaunchDescription([ + sim_launch, + pd_ctrl_node, + ReadyToTest(), + ]), {'sim_launch': sim_launch, 'pd_ctrl_node': pd_ctrl_node} + + +class TestPDControlNode(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_pd_ctrl_node') + + def tearDown(self): + self.node.destroy_node() + + def test_set_target_state(self, launch_service): + current_state = FreeFlyerStateStamped() + + def state_callback(msg): + current_state.header = msg.header + current_state.state = msg.state + + sub = self.node.create_subscription( + FreeFlyerStateStamped, + f'/{ROBOT_NAME}/gt/state', + state_callback, + 10, + ) + pub = self.node.create_publisher( + FreeFlyerStateStamped, + f'/{ROBOT_NAME}/ctrl/state', + 10, + ) + + try: + # wait for nodes to start up (with 5 seconds timeout) + end_time = time.time() + 5. + node_flag = False + while time.time() < end_time and not node_flag: + node_flag = 'pd_ctrl_node' in self.node.get_node_names() and \ + 'simulator_node' in self.node.get_node_names() + time.sleep(0.1) + assert node_flag, 'pd_ctrl_node or simualtor_node launch failure' + + # wait for node to initialize + time.sleep(3.) + + # publish target state + target_state = FreeFlyerStateStamped() + target_state.header.stamp = self.node.get_clock().now().to_msg() + target_state.state.pose.x = 1. + target_state.state.pose.y = 1. + target_state.state.pose.theta = 1. + pub.publish(target_state) + + # wait for 15 seconds and check results + end_time = time.time() + 15. + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + + # current state should be close to the target state + self.assertAlmostEquals(current_state.state.pose.x, 1., delta=1e-1) + self.assertAlmostEquals(current_state.state.pose.y, 1., delta=1e-1) + self.assertAlmostEquals(current_state.state.pose.theta, 1., delta=1e-1) + + finally: + self.node.destroy_subscription(sub) + self.node.destroy_publisher(pub) From 15b992a68e8be7731570d751330eca7240b72ce1 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 21:36:33 -0700 Subject: [PATCH 39/56] fix Python style errors --- ff_control/ff_control/__init__.py | 21 +++++++ ff_control/ff_control/linear_ctrl.py | 92 +++++++++++++++------------- ff_control/ff_control/ll_ctrl.py | 50 ++++++++++----- ff_control/ff_control/wrench_ctrl.py | 61 +++++++++++------- ff_control/scripts/pd_ctrl_py_node | 47 ++++++++++---- 5 files changed, 182 insertions(+), 89 deletions(-) diff --git a/ff_control/ff_control/__init__.py b/ff_control/ff_control/__init__.py index e69de29..23995af 100644 --- a/ff_control/ff_control/__init__.py +++ b/ff_control/ff_control/__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_control/ff_control/linear_ctrl.py b/ff_control/ff_control/linear_ctrl.py index 4f5588e..1a9830d 100644 --- a/ff_control/ff_control/linear_ctrl.py +++ b/ff_control/ff_control/linear_ctrl.py @@ -1,19 +1,40 @@ -import rclpy +# 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 copy -import numpy as np import typing as T from ff_control.wrench_ctrl import WrenchController -from ff_msgs.msg import Wrench2D from ff_msgs.msg import FreeFlyerState from ff_msgs.msg import FreeFlyerStateStamped -from ff_params import RobotParams +from ff_msgs.msg import Wrench2D + +import numpy as np class LinearController(WrenchController): """ - base class for any linear controller + Base class for any linear controller. state definition: [x, y, theta, vx, vy, wz] control definition: [fx, fy, tz] @@ -25,47 +46,39 @@ class LinearController(WrenchController): STATE_DIM = 6 CONTROL_DIM = 3 - def __init__(self, node_name="linear_ctrl_node"): + def __init__(self, node_name='linear_ctrl_node'): super().__init__(node_name) self._state_sub = self.create_subscription(FreeFlyerStateStamped, - "gt/state", self._state_callback, 10) + 'gt/state', self._state_callback, 10) self._state_ready = False self._state_stamped = FreeFlyerStateStamped() @property def feedback_gain_shape(self) -> T.Tuple[int, int]: - """get shape of the feedback control gain matrix - - Returns: - T.Tuple[int, int]: (CONTROL_DIM, STATE_DIM) - """ + """Get shape of the feedback control gain matrix.""" return (self.CONTROL_DIM, self.STATE_DIM) def get_state(self) -> T.Optional[FreeFlyerState]: - """get the current latest state - - Returns: - T.Optional[FreeFlyerState]: the current state, None if not available - """ + """Get the current latest state.""" if not self._state_ready: - self.get_logger().error("get_state failed: state not yet ready") + self.get_logger().error('get_state failed: state not yet ready') return None return self._state_stamped.state def send_control(self, state_des: T.Union[FreeFlyerState, np.ndarray], K: np.ndarray) -> None: - """send desirable target state for linear control + """ + Send desirable target state for linear control. - Args: - state_des (T.Union[FreeFlyerState, np.ndarray]): desired state - K (np.ndarray): feedback control matrix (i.e. u = Kx) + :param state_des: desired state + :param K: feedback control matrix (i.e. u = Kx) """ if not self._state_ready: - self.get_logger().warn("send_control ignored, state not yet ready") + self.get_logger().warn('send_control ignored, state not yet ready') return if K.shape != self.feedback_gain_shape: - self.get_logger().error("send_control failed: incompatible gain matrix shape") + self.get_logger().error('send_control failed: incompatible gain matrix shape') return # convert desired state to vector form @@ -86,20 +99,20 @@ def send_control(self, state_des: T.Union[FreeFlyerState, np.ndarray], K: np.nda self.set_world_wrench(wrench_world, state_vector[2]) def state_ready_callback(self) -> None: - """callback invoked when the first state measurement comes in + """ + Get called when the first state measurement comes in. + Sub-classes should override this function """ pass @staticmethod def state2vec(state: FreeFlyerState) -> np.ndarray: - """convert state message to state vector - - Args: - state (FreeFlyerState): state message + """ + Convert state message to state vector. - Returns: - np.ndarray: state vector + :param state: state message + :return: state vector """ return np.array([ state.pose.x, @@ -112,13 +125,11 @@ def state2vec(state: FreeFlyerState) -> np.ndarray: @staticmethod def vec2state(vec: np.ndarray) -> FreeFlyerState: - """convert state vector to state message - - Args: - vec (np.ndarray): state vector + """ + Convert state vector to state message. - Returns: - FreeFlyerState: state message + :param vec: state vector + :return: state message """ state = FreeFlyerState() state.pose.x = vec[0] @@ -131,10 +142,10 @@ def vec2state(vec: np.ndarray) -> FreeFlyerState: return state def state_is_ready(self) -> bool: - """check if state is ready + """ + Check if state is ready. - Returns: - bool: True if state is ready, False otherwise + :return: True if state is ready, False otherwise """ return self._state_ready @@ -144,4 +155,3 @@ def _state_callback(self, msg: FreeFlyerStateStamped) -> None: if not self._state_ready: self._state_ready = True self.state_ready_callback() - diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index 6b9201d..a715913 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -1,33 +1,54 @@ -import rclpy -from rclpy.node import Node - -import numpy as np +# 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 ff_msgs.msg import ThrusterCommand from ff_msgs.msg import WheelVelCommand from ff_params import RobotParams +import numpy as np + +from rclpy.node import Node + class LowLevelController(Node): - def __init__(self, node_name: str = "ll_ctrl_node") -> None: + def __init__(self, node_name: str = 'll_ctrl_node') -> None: super().__init__(node_name) # robot parameters that can be accessed by sub-classes self.p = RobotParams(self) # low level control publishers - self._thruster_pub = self.create_publisher(ThrusterCommand, "commands/duty_cycle", 10) - self._wheel_pub = self.create_publisher(WheelVelCommand, "commands/velocity", 10) + self._thruster_pub = self.create_publisher(ThrusterCommand, 'commands/duty_cycle', 10) + self._wheel_pub = self.create_publisher(WheelVelCommand, 'commands/velocity', 10) def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: - """send command to set the thrusters duty cycles + """ + Send command to set the thrusters duty cycles. - Args: - duty_cycle (np.ndarray): duty cycle for each thrust (in [0, 1]) + :param duty_cycle: duty cycle for each thrust (in [0, 1]) """ if len(duty_cycle) != len(ThrusterCommand().duty_cycle): - self.get_logger().error("Incompatible thruster length sent.") + self.get_logger().error('Incompatible thruster length sent.') return msg = ThrusterCommand() msg.header.stamp = self.get_clock().now().to_msg() @@ -35,15 +56,14 @@ def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: self._thruster_pub.publish(msg) def set_wheel_velocity(self, velocity: float) -> None: - """send command to set the inertial wheel velocity + """ + Send command to set the inertial wheel velocity. TODO(alvin): suppor this or remove? - Args: - velocity (float): angular velocity in [rad/s] + :param velocity: angular velocity in [rad/s] """ msg = WheelVelCommand() msg.header.stamp = self.get_clock().now().to_msg() msg.velocity = velocity self._wheel_pub.publish(msg) - diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index 17490cd..764b612 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -1,37 +1,56 @@ -import numpy as np - -import rclpy +# 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 ff_control.ll_ctrl import LowLevelController from ff_msgs.msg import Wrench2D +import numpy as np + class WrenchController(LowLevelController): - def __init__(self, node_name: str = "wrench_ctrl_node") -> None: + def __init__(self, node_name: str = 'wrench_ctrl_node') -> None: super().__init__(node_name) def set_body_wrench(self, wrench_body: Wrench2D, use_wheel: bool = False) -> None: - """set wrench in body frame + """ + Set wrench in body frame. - Args: - wrench_body (Wrench2D): wrench in body frame - use_wheel (bool): set to ture to use the inertial wheel - (TODO(alvin): unsupported) + :param wrench_body: wrench in body frame + :param use_wheel: set to ture to use the inertial wheel (TODO(alvin): unsupported) """ if use_wheel: - self.get_logger().error("set_wrench failed: use_wheel not implemented") + self.get_logger().error('set_wrench failed: use_wheel not implemented') return duty_cycle = np.zeros(8) wrench_body_clipped = self.clip_wrench(wrench_body) # convert force - u_Fx = wrench_body_clipped.fx / (2 * self.p.actuators["F_max_per_thruster"]) - u_Fy = wrench_body_clipped.fy / (2 * self.p.actuators["F_max_per_thruster"]) + u_Fx = wrench_body_clipped.fx / (2 * self.p.actuators['F_max_per_thruster']) + u_Fy = wrench_body_clipped.fy / (2 * self.p.actuators['F_max_per_thruster']) if u_Fx > 0: duty_cycle[2] = u_Fx - duty_cycle[5] = u_Fx; + duty_cycle[5] = u_Fx else: duty_cycle[1] = -u_Fx duty_cycle[6] = -u_Fx @@ -43,8 +62,8 @@ def set_body_wrench(self, wrench_body: Wrench2D, use_wheel: bool = False) -> Non duty_cycle[3] = -u_Fy # convert torque - u_M = wrench_body_clipped.tz / (4 * self.p.actuators["F_max_per_thruster"] \ - * self.p.actuators["thrusters_lever_arm"]) + u_M = wrench_body_clipped.tz / (4 * self.p.actuators['F_max_per_thruster'] + * self.p.actuators['thrusters_lever_arm']) if u_M > 0: duty_cycle[[1, 3, 5, 7]] += u_M else: @@ -56,11 +75,11 @@ def set_body_wrench(self, wrench_body: Wrench2D, use_wheel: bool = False) -> Non self.set_thrust_duty_cycle(duty_cycle) def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: - """set wrench in world frame + """ + Set wrench in world frame. - Args: - wrench_world (Wrench2D): wrench in world frame - theta (float): rotational state of the freeflyer + :param wrench_world: wrench in world frame + :param theta: rotational state of the freeflyer """ cos_theta = np.cos(theta) sin_theta = np.sin(theta) @@ -75,8 +94,8 @@ def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: def clip_wrench(self, wrench: Wrench2D) -> Wrench2D: wrench_clipped = Wrench2D() force = np.sqrt(wrench.fx**2 + wrench.fy**2) - force_scale = max(force / self.p.actuators["F_body_max"], 1.0) - torque_scale = max(abs(wrench.tz) / self.p.actuators["M_body_max"], 1.0) + force_scale = max(force / self.p.actuators['F_body_max'], 1.0) + torque_scale = max(abs(wrench.tz) / self.p.actuators['M_body_max'], 1.0) wrench_clipped.fx = wrench.fx / force_scale wrench_clipped.fy = wrench.fy / force_scale diff --git a/ff_control/scripts/pd_ctrl_py_node b/ff_control/scripts/pd_ctrl_py_node index 7e1745e..6f9bbfb 100755 --- a/ff_control/scripts/pd_ctrl_py_node +++ b/ff_control/scripts/pd_ctrl_py_node @@ -1,3 +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. + + #!/usr/bin/env python3 import copy @@ -13,19 +36,19 @@ from geometry_msgs.msg import PoseStamped class PDControlNode(LinearController): def __init__(self): - super().__init__("pd_control_node") + super().__init__('pd_control_node') self.state_sp_sub = self.create_subscription(FreeFlyerStateStamped, - "ctrl/state", self.state_setpoint_callback, 10) + 'ctrl/state', self.state_setpoint_callback, 10) self.rviz_sp_sub = self.create_subscription(PoseStamped, - "/goal_pose", self.rviz_setpoint_callback, 10) + '/goal_pose', self.rviz_setpoint_callback, 10) self.timer = self.create_timer(0.1, self.control_loop) self.state_desired = FreeFlyerStateStamped() # feedback gain params - self.declare_parameter("gain_f", 2.0) - self.declare_parameter("gain_df", 10.0) - self.declare_parameter("gain_t", 0.2) - self.declare_parameter("gain_dt", 0.4) + self.declare_parameter('gain_f', 2.0) + self.declare_parameter('gain_df', 10.0) + self.declare_parameter('gain_t', 0.2) + self.declare_parameter('gain_dt', 0.4) def state_ready_callback(self) -> None: # copy current position as goal position @@ -54,10 +77,10 @@ class PDControlNode(LinearController): return # build feedback gain matrix - gain_f = self.get_parameter("gain_f").get_parameter_value().double_value - gain_df = self.get_parameter("gain_df").get_parameter_value().double_value - gain_t = self.get_parameter("gain_t").get_parameter_value().double_value - gain_dt = self.get_parameter("gain_dt").get_parameter_value().double_value + gain_f = self.get_parameter('gain_f').get_parameter_value().double_value + gain_df = self.get_parameter('gain_df').get_parameter_value().double_value + gain_t = self.get_parameter('gain_t').get_parameter_value().double_value + gain_dt = self.get_parameter('gain_dt').get_parameter_value().double_value K = np.array([[gain_f, 0, 0, gain_df, 0, 0], [0, gain_f, 0, 0, gain_df, 0], [0, 0, gain_t, 0, 0, gain_dt]]) @@ -71,5 +94,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main() From e57ff992212864049628a46728160e597a5d5f45 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 21:41:46 -0700 Subject: [PATCH 40/56] update test for cpp node --- freeflyer/test/pd_ctrl_launch_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py index 6f4ac58..dc2235f 100644 --- a/freeflyer/test/pd_ctrl_launch_test.py +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -50,7 +50,7 @@ def generate_test_description(): ) pd_ctrl_node = Node( package='ff_control', - executable='pd_ctrl_node', + executable='pd_ctrl_cpp_node', name='pd_ctrl_node', namespace=ROBOT_NAME, ) From 42dc57743798946bf1ceae5686ba968f573f5ddb Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 22:13:15 -0700 Subject: [PATCH 41/56] test python implementation of pd control node --- ff_control/scripts/pd_ctrl_py_node | 4 ++-- ff_params/ff_params/__init__.py | 3 ++- freeflyer/CMakeLists.txt | 5 ++++- freeflyer/test/pd_ctrl_launch_test.py | 13 ++++++++----- 4 files changed, 16 insertions(+), 9 deletions(-) diff --git a/ff_control/scripts/pd_ctrl_py_node b/ff_control/scripts/pd_ctrl_py_node index 6f9bbfb..8e651a8 100755 --- a/ff_control/scripts/pd_ctrl_py_node +++ b/ff_control/scripts/pd_ctrl_py_node @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + # MIT License # # Copyright (c) 2023 Stanford Autonomous Systems Lab @@ -21,8 +23,6 @@ # SOFTWARE. -#!/usr/bin/env python3 - import copy import rclpy diff --git a/ff_params/ff_params/__init__.py b/ff_params/ff_params/__init__.py index a14dfcb..9739e45 100644 --- a/ff_params/ff_params/__init__.py +++ b/ff_params/ff_params/__init__.py @@ -26,11 +26,12 @@ import numpy as np from rcl_interfaces.srv import GetParameters import rclpy +from rclpy.node import Node class RobotParams: def __init__(self, - node: rclpy.node.Node, + node: Node, param_ready_cb: T.Optional[T.Callable[[], None]] = None): robot_name = node.get_namespace() cli = node.create_client(GetParameters, f'{robot_name}/robot_params_node/get_parameters') diff --git a/freeflyer/CMakeLists.txt b/freeflyer/CMakeLists.txt index a55d074..69806e7 100644 --- a/freeflyer/CMakeLists.txt +++ b/freeflyer/CMakeLists.txt @@ -20,7 +20,10 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) ament_lint_auto_find_test_dependencies() - add_launch_test(test/pd_ctrl_launch_test.py) + + # test both implementations of PD controller + add_launch_test(test/pd_ctrl_launch_test.py TARGET pd_ctrl_cpp_test ARGS "impl:=cpp") + add_launch_test(test/pd_ctrl_launch_test.py TARGET pd_ctrl_py_test ARGS "impl:=py") endif() ament_package() diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py index dc2235f..51c592b 100644 --- a/freeflyer/test/pd_ctrl_launch_test.py +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -27,8 +27,8 @@ from ff_msgs.msg import FreeFlyerStateStamped from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest @@ -40,6 +40,8 @@ def generate_test_description(): + impl = LaunchConfiguration('impl') + sim_launch = IncludeLaunchDescription( PathJoinSubstitution([ FindPackageShare('ff_sim'), @@ -50,16 +52,17 @@ def generate_test_description(): ) pd_ctrl_node = Node( package='ff_control', - executable='pd_ctrl_cpp_node', + executable=['pd_ctrl_', impl, '_node'], name='pd_ctrl_node', namespace=ROBOT_NAME, ) return LaunchDescription([ + DeclareLaunchArgument('impl', default_value='cpp', choices=['cpp', 'py']), sim_launch, pd_ctrl_node, ReadyToTest(), - ]), {'sim_launch': sim_launch, 'pd_ctrl_node': pd_ctrl_node} + ]) class TestPDControlNode(unittest.TestCase): @@ -78,7 +81,7 @@ def setUp(self): def tearDown(self): self.node.destroy_node() - def test_set_target_state(self, launch_service): + def test_set_target_state(self): current_state = FreeFlyerStateStamped() def state_callback(msg): From a11cf6d644f62f43252e2b17a563138140656a8f Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 23:51:40 -0700 Subject: [PATCH 42/56] fix freeflyer test failure --- freeflyer/launch/estimation_viz.launch.py | 48 +++++++++++++++++------ freeflyer/launch/sim_key_teleop.launch.py | 6 +-- freeflyer/launch/sim_pd.launch.py | 6 +-- freeflyer/test/pd_ctrl_launch_test.py | 11 +++++- 4 files changed, 51 insertions(+), 20 deletions(-) diff --git a/freeflyer/launch/estimation_viz.launch.py b/freeflyer/launch/estimation_viz.launch.py index 8babe9e..b3e7626 100644 --- a/freeflyer/launch/estimation_viz.launch.py +++ b/freeflyer/launch/estimation_viz.launch.py @@ -1,32 +1,56 @@ +# 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") + robot_name = LaunchConfiguration('robot_name') return LaunchDescription([ - DeclareLaunchArgument("robot_name", default_value="robot"), + DeclareLaunchArgument('robot_name', default_value='robot'), IncludeLaunchDescription( PathJoinSubstitution([ - FindPackageShare("ff_viz"), - "launch", - "ff_viz.launch.py", + FindPackageShare('ff_viz'), + 'launch', + 'ff_viz.launch.py', ]), - launch_arguments={"robot_name": robot_name}.items(), + launch_arguments={'robot_name': robot_name}.items(), ), Node( - package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + package='ff_estimate', + executable='mocap_estimator_node', + name='mocap_estimator_node', namespace=robot_name, parameters=[{ - "pose_channel": PathJoinSubstitution([ - "mocap", + 'pose_channel': PathJoinSubstitution([ + 'mocap', robot_name, - "pose", + 'pose', ]), }], ), diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index 589229b..986a189 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -57,9 +57,9 @@ def generate_launch_description(): prefix='gnome-terminal --', ), Node( - package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + package='ff_estimate', + executable='mocap_estimator_node', + name='mocap_estimator_node', namespace=robot_name, ), ]) diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 07c418e..3867b3f 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -56,9 +56,9 @@ def generate_launch_description(): namespace=robot_name, ), Node( - package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + package='ff_estimate', + executable='mocap_estimator_node', + name='mocap_estimator_node', namespace=robot_name, ), ]) diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py index 6f4ac58..badf98b 100644 --- a/freeflyer/test/pd_ctrl_launch_test.py +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -54,12 +54,19 @@ def generate_test_description(): name='pd_ctrl_node', namespace=ROBOT_NAME, ) + mocap_estimator = Node( + package='ff_estimate', + executable='mocap_estimator_node', + name='mocap_estimator_node', + namespace=ROBOT_NAME, + ) return LaunchDescription([ sim_launch, pd_ctrl_node, + mocap_estimator, ReadyToTest(), - ]), {'sim_launch': sim_launch, 'pd_ctrl_node': pd_ctrl_node} + ]) class TestPDControlNode(unittest.TestCase): @@ -87,7 +94,7 @@ def state_callback(msg): sub = self.node.create_subscription( FreeFlyerStateStamped, - f'/{ROBOT_NAME}/gt/state', + f'/{ROBOT_NAME}/sim/state', state_callback, 10, ) From 730a8b07e8c3af45c69ee31a23cb8b82d235de15 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 17 May 2023 23:54:58 -0700 Subject: [PATCH 43/56] fix ff_estimate test failure --- ff_estimate/CMakeLists.txt | 7 --- .../include/ff_estimate/base_estimator.hpp | 41 ++++++++++++++---- .../include/ff_estimate/pose2d_estimator.hpp | 43 +++++++++++++++---- ff_estimate/src/base_estimator.cpp | 35 +++++++++++++-- ff_estimate/src/mocap_estimator_node.cpp | 40 ++++++++++++++--- ff_estimate/src/pose2d_estimator.cpp | 36 +++++++++++++--- 6 files changed, 164 insertions(+), 38 deletions(-) diff --git a/ff_estimate/CMakeLists.txt b/ff_estimate/CMakeLists.txt index d12685f..acfbdaa 100644 --- a/ff_estimate/CMakeLists.txt +++ b/ff_estimate/CMakeLists.txt @@ -46,13 +46,6 @@ install(TARGETS mocap_estimator_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_estimate/include/ff_estimate/base_estimator.hpp b/ff_estimate/include/ff_estimate/base_estimator.hpp index acdfd13..62585f5 100644 --- a/ff_estimate/include/ff_estimate/base_estimator.hpp +++ b/ff_estimate/include/ff_estimate/base_estimator.hpp @@ -1,3 +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. + + #pragma once #include @@ -7,25 +30,27 @@ #include "ff_msgs/msg/free_flyer_state.hpp" #include "ff_msgs/msg/free_flyer_state_stamped.hpp" -namespace ff { +namespace ff +{ /** * @brief estimator base class */ -class BaseEstimator : public rclcpp::Node { - public: - BaseEstimator(const std::string& node_name); +class BaseEstimator : public rclcpp::Node +{ +public: + explicit BaseEstimator(const std::string & node_name); - protected: +protected: /** * @brief send out the current state estimate * * @param state estimated freeflyer state */ - void SendStateEstimate(const ff_msgs::msg::FreeFlyerState& state); + void SendStateEstimate(const ff_msgs::msg::FreeFlyerState & state); - private: +private: rclcpp::Publisher::SharedPtr state_pub_; }; -} // namespace ff +} // namespace ff diff --git a/ff_estimate/include/ff_estimate/pose2d_estimator.hpp b/ff_estimate/include/ff_estimate/pose2d_estimator.hpp index 250ce0f..2b8077d 100644 --- a/ff_estimate/include/ff_estimate/pose2d_estimator.hpp +++ b/ff_estimate/include/ff_estimate/pose2d_estimator.hpp @@ -1,29 +1,56 @@ +// 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. + + #pragma once +#include + #include "ff_estimate/base_estimator.hpp" #include "ff_msgs/msg/free_flyer_state_stamped.hpp" #include "ff_msgs/msg/pose2_d_stamped.hpp" -namespace ff { +namespace ff +{ /** * @brief full-state estimator using only pose measurements */ -class Pose2DEstimator: public BaseEstimator { - public: - Pose2DEstimator(const std::string& node_name); +class Pose2DEstimator : public BaseEstimator +{ +public: + explicit Pose2DEstimator(const std::string & node_name); - protected: +protected: /** * @brief estimate state with Pose2D * * @param pose_stamped timestamped pose2d */ - virtual void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped& pose_stamped); + virtual void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped & pose_stamped); - private: +private: ff_msgs::msg::FreeFlyerStateStamped prev_; bool prev_state_ready_ = false; }; -} // namespace ff +} // namespace ff diff --git a/ff_estimate/src/base_estimator.cpp b/ff_estimate/src/base_estimator.cpp index d22c215..a53df2c 100644 --- a/ff_estimate/src/base_estimator.cpp +++ b/ff_estimate/src/base_estimator.cpp @@ -1,15 +1,42 @@ +// 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. + + #include "ff_estimate/base_estimator.hpp" using ff_msgs::msg::FreeFlyerState; using ff_msgs::msg::FreeFlyerStateStamped; -namespace ff { +namespace ff +{ -BaseEstimator::BaseEstimator(const std::string& node_name) : rclcpp::Node(node_name) { +BaseEstimator::BaseEstimator(const std::string & node_name) +: rclcpp::Node(node_name) +{ state_pub_ = this->create_publisher("est/state", 10); } -void BaseEstimator::SendStateEstimate(const FreeFlyerState& state) { +void BaseEstimator::SendStateEstimate(const FreeFlyerState & state) +{ ff_msgs::msg::FreeFlyerStateStamped msg{}; msg.state = state; msg.header.stamp = this->get_clock()->now(); @@ -17,4 +44,4 @@ void BaseEstimator::SendStateEstimate(const FreeFlyerState& state) { state_pub_->publish(msg); } -} // namespace ff +} // namespace ff diff --git a/ff_estimate/src/mocap_estimator_node.cpp b/ff_estimate/src/mocap_estimator_node.cpp index 00bb363..92b35fb 100644 --- a/ff_estimate/src/mocap_estimator_node.cpp +++ b/ff_estimate/src/mocap_estimator_node.cpp @@ -1,3 +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. + + #include #include #include @@ -10,18 +33,22 @@ using ff_msgs::msg::Pose2DStamped; using geometry_msgs::msg::PoseStamped; -class MocapEstimatorNode : public ff::Pose2DEstimator { - public: - MocapEstimatorNode() : ff::Pose2DEstimator("mocap_estimator_node") { +class MocapEstimatorNode : public ff::Pose2DEstimator +{ +public: + MocapEstimatorNode() + : ff::Pose2DEstimator("mocap_estimator_node") + { const std::string pose_channel = this->declare_parameter("pose_channel", "mocap/sim/pose"); pose_sub_ = this->create_subscription( pose_channel, 10, [this](const PoseStamped::SharedPtr msg) {PoseCallback(msg);}); } - private: +private: rclcpp::Subscription::SharedPtr pose_sub_; - void PoseCallback(const PoseStamped::SharedPtr pose) { + void PoseCallback(const PoseStamped::SharedPtr pose) + { // convert to pose2d Pose2DStamped pose2d{}; @@ -36,7 +63,8 @@ class MocapEstimatorNode : public ff::Pose2DEstimator { } }; -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/ff_estimate/src/pose2d_estimator.cpp b/ff_estimate/src/pose2d_estimator.cpp index 6f4e297..c06abe9 100644 --- a/ff_estimate/src/pose2d_estimator.cpp +++ b/ff_estimate/src/pose2d_estimator.cpp @@ -1,20 +1,46 @@ +// 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. + + #include "ff_estimate/pose2d_estimator.hpp" #include "ff_msgs/msg/free_flyer_state.hpp" using ff_msgs::msg::FreeFlyerState; using ff_msgs::msg::Pose2DStamped; -namespace ff { +namespace ff +{ -Pose2DEstimator::Pose2DEstimator(const std::string& node_name) - : BaseEstimator(node_name) { +Pose2DEstimator::Pose2DEstimator(const std::string & node_name) +: BaseEstimator(node_name) +{ // perform simple 1st order IIR lowpass filter on derivate estimates // coefficient in [0, 1) (higher coefficient filters better with larger delay) this->declare_parameter("lowpass_coeff", .95); this->declare_parameter("min_dt", 0.005); } -void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped& pose_stamped) { +void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped & pose_stamped) +{ FreeFlyerState state{}; state.pose = pose_stamped.pose; @@ -53,4 +79,4 @@ void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped& pose_stamped) { SendStateEstimate(state); } -} // namespace ff +} // namespace ff From 51ab91bb2f983b76352a9e020c3e1f997feb0c77 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 18 May 2023 10:03:58 -0700 Subject: [PATCH 44/56] disable all flake8 and pep257 linter in ament_lint --- ff_control/CMakeLists.txt | 2 ++ ff_params/CMakeLists.txt | 2 ++ ff_viz/CMakeLists.txt | 2 ++ freeflyer/CMakeLists.txt | 3 +++ 4 files changed, 9 insertions(+) diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index d1e07cc..337357d 100644 --- a/ff_control/CMakeLists.txt +++ b/ff_control/CMakeLists.txt @@ -54,6 +54,8 @@ install(TARGETS wrench_ctrl_node pd_ctrl_node key_teleop_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + # disable Python style checkers + set(AMENT_LINT_AUTO_EXCLUDE ament_cmake_flake8 ament_cmake_pep257) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_params/CMakeLists.txt b/ff_params/CMakeLists.txt index 8271048..9365b5f 100644 --- a/ff_params/CMakeLists.txt +++ b/ff_params/CMakeLists.txt @@ -45,6 +45,8 @@ ament_python_install_package(${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + # disable Python style checkers + set(AMENT_LINT_AUTO_EXCLUDE ament_cmake_flake8 ament_cmake_pep257) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_viz/CMakeLists.txt b/ff_viz/CMakeLists.txt index 604efb8..e71c5ef 100644 --- a/ff_viz/CMakeLists.txt +++ b/ff_viz/CMakeLists.txt @@ -34,6 +34,8 @@ install(TARGETS renderer_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + # disable Python style checkers + set(AMENT_LINT_AUTO_EXCLUDE ament_cmake_flake8 ament_cmake_pep257) ament_lint_auto_find_test_dependencies() endif() diff --git a/freeflyer/CMakeLists.txt b/freeflyer/CMakeLists.txt index a55d074..1b2608a 100644 --- a/freeflyer/CMakeLists.txt +++ b/freeflyer/CMakeLists.txt @@ -19,7 +19,10 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + # disable Python style checkers + set(AMENT_LINT_AUTO_EXCLUDE ament_cmake_flake8 ament_cmake_pep257) ament_lint_auto_find_test_dependencies() + add_launch_test(test/pd_ctrl_launch_test.py) endif() From bcd457912da05b2e51aeb0e00c21b2663196edf9 Mon Sep 17 00:00:00 2001 From: Robert Dyro Date: Thu, 18 May 2023 10:54:59 -0700 Subject: [PATCH 45/56] adding a black formatting test to ensure code formatting convention & formatted python files to pass that test --- .gitignore | 3 + ff_params/ff_params/__init__.py | 66 ++++---- ff_sim/ff_sim/simulator_node.py | 181 ++++++++++++---------- ff_sim/launch/single.launch.py | 33 ++-- ff_sim/setup.py | 27 ++-- ff_sim/test/test_copyright.py | 4 +- ff_viz/launch/ff_viz.launch.py | 69 +++++---- freeflyer/CMakeLists.txt | 2 + freeflyer/launch/sim_key_teleop.launch.py | 60 +++---- freeflyer/launch/sim_pd.launch.py | 58 +++---- freeflyer/test/black_formatting.py | 45 ++++++ freeflyer/test/pd_ctrl_launch_test.py | 67 ++++---- pyproject.toml | 17 ++ 13 files changed, 370 insertions(+), 262 deletions(-) create mode 100644 freeflyer/test/black_formatting.py create mode 100644 pyproject.toml diff --git a/.gitignore b/.gitignore index b6e4761..6a39dbd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ +.vscode/* +.history/ + # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] diff --git a/ff_params/ff_params/__init__.py b/ff_params/ff_params/__init__.py index a14dfcb..b93a9ff 100644 --- a/ff_params/ff_params/__init__.py +++ b/ff_params/ff_params/__init__.py @@ -29,33 +29,33 @@ class RobotParams: - def __init__(self, - node: rclpy.node.Node, - param_ready_cb: T.Optional[T.Callable[[], None]] = None): + def __init__( + self, node: rclpy.node.Node, param_ready_cb: T.Optional[T.Callable[[], None]] = None + ): robot_name = node.get_namespace() - cli = node.create_client(GetParameters, f'{robot_name}/robot_params_node/get_parameters') + cli = node.create_client(GetParameters, f"{robot_name}/robot_params_node/get_parameters") self.node = node self.param_ready_cb = param_ready_cb while not cli.service_is_ready(): - cli.wait_for_service(5.) - node.get_logger().info('parameter service not ready, retrying...') + cli.wait_for_service(5.0) + node.get_logger().info("parameter service not ready, retrying...") self.request = GetParameters.Request() self.request.names = [ - 'dynamics.mass', - 'dynamics.inertia', - 'dynamics.radius', - 'dynamics.CoM_offset', - 'dynamics.force_const', - 'actuators.F_max_per_thruster', - 'actuators.thrusters_lever_arm', - 'actuators.F_body_max', - 'actuators.M_body_max', - 'actuators.min_inp_percent', - 'actuators.max_inp_percent', - 'actuators.gamma_min', - 'actuators.gamma_max', + "dynamics.mass", + "dynamics.inertia", + "dynamics.radius", + "dynamics.CoM_offset", + "dynamics.force_const", + "actuators.F_max_per_thruster", + "actuators.thrusters_lever_arm", + "actuators.F_body_max", + "actuators.M_body_max", + "actuators.min_inp_percent", + "actuators.max_inp_percent", + "actuators.gamma_min", + "actuators.gamma_max", ] future = cli.call_async(self.request) @@ -67,25 +67,25 @@ def _param_ready_cb(self, future: rclpy.task.Future): try: res = future.result() except Exception as e: - self.node.get_logger().error('get robot params failed: %r' % (e,)) + self.node.get_logger().error("get robot params failed: %r" % (e,)) else: self.dynamics = { - 'mass': res.values[0].double_value, - 'inertia': res.values[1].double_value, - 'radius': res.values[2].double_value, - 'CoM_offset': np.array(res.values[3].double_array_value), - 'force_const': np.array(res.values[4].double_array_value), + "mass": res.values[0].double_value, + "inertia": res.values[1].double_value, + "radius": res.values[2].double_value, + "CoM_offset": np.array(res.values[3].double_array_value), + "force_const": np.array(res.values[4].double_array_value), } self.actuators = { - 'F_max_per_thruster': res.values[5].double_value, - 'thrusters_lever_arm': res.values[6].double_value, - 'F_body_max': res.values[7].double_value, - 'M_body_max': res.values[8].double_value, - 'min_inp_percent': res.values[9].double_value, - 'max_inp_percent': res.values[10].double_value, - 'gamma_min': res.values[11].double_value, - 'gamma_max': res.values[12].double_value, + "F_max_per_thruster": res.values[5].double_value, + "thrusters_lever_arm": res.values[6].double_value, + "F_body_max": res.values[7].double_value, + "M_body_max": res.values[8].double_value, + "min_inp_percent": res.values[9].double_value, + "max_inp_percent": res.values[10].double_value, + "gamma_min": res.values[11].double_value, + "gamma_max": res.values[12].double_value, } self.loaded = True diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index f61402b..63a15a9 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -40,10 +40,9 @@ from ff_params import RobotParams + def simulate_contact( - state: np.ndarray, - robot_radius: float, - obstacles: T.Dict[str, T.Any] + state: np.ndarray, robot_radius: float, obstacles: T.Dict[str, T.Any] ) -> T.Tuple[np.ndarray, np.ndarray, bool]: """ Simulate contact with obstacles. @@ -63,39 +62,44 @@ def simulate_contact( """ B_contact = False - F_MAG_NOM = 6. # in [N] + F_MAG_NOM = 6.0 # in [N] - radius = robot_radius + radius = robot_radius p_x, p_y, theta, v_x, v_y, omega = state # check for contacts with cylinders - n_obs_cyl = len(obstacles['cyl_pos_x']) + n_obs_cyl = len(obstacles["cyl_pos_x"]) for o_i in range(n_obs_cyl): - o_x, o_y, o_r = obstacles['cyl_pos_x'][o_i], obstacles['cyl_pos_y'][o_i], obstacles['cyl_rads'][o_i] - if (p_x-o_x)**2 + (p_y-o_y)**2 < (radius+o_r)**2: - print("Contact with obstacle " + str(o_i) + " !" ) + o_x, o_y, o_r = ( + obstacles["cyl_pos_x"][o_i], + obstacles["cyl_pos_y"][o_i], + obstacles["cyl_rads"][o_i], + ) + if (p_x - o_x) ** 2 + (p_y - o_y) ** 2 < (radius + o_r) ** 2: + print("Contact with obstacle " + str(o_i) + " !") B_contact = True # normal direction (pointing outside obstacle) - F_dir = np.array([p_x-o_x, p_y-o_y]) + F_dir = np.array([p_x - o_x, p_y - o_y]) F_dir = F_dir / np.linalg.norm(F_dir) # make position lie on boundary of obstacle - state[0:2] = np.array([o_x, o_y]) - state[0:2] += F_dir*(radius+o_r+1e-3) - state[ 2 ] = state[2] + state[0:2] = np.array([o_x, o_y]) + state[0:2] += F_dir * (radius + o_r + 1e-3) + state[2] = state[2] # bounce with vel. in opposite direction (eq. (13) of https://arxiv.org/pdf/1909.00079.pdf) - state[3:5] += -2. * np.dot(state[3:5],F_dir)*F_dir - state[ 5 ] = -state[5] + state[3:5] += -2.0 * np.dot(state[3:5], F_dir) * F_dir + state[5] = -state[5] # External contact wrench (N,N,N*m) - W_contact = np.array([F_MAG_NOM*F_dir[0], F_MAG_NOM*F_dir[1], 0.]) + W_contact = np.array([F_MAG_NOM * F_dir[0], F_MAG_NOM * F_dir[1], 0.0]) return state, W_contact, B_contact W_contact = np.zeros(3) return state, W_contact, B_contact + class FreeFlyerSimulator(Node): """Free-Flyer simulator class.""" @@ -106,12 +110,15 @@ def __init__(self): self.p = RobotParams(self) # obstacles - p_obstacles = self.declare_parameters("obstacles",[ - ("cyl_pos_x", []), - ("cyl_pos_y", []), - ("cyl_rads", []), - ("cyl_heights", []), - ]) + p_obstacles = self.declare_parameters( + "obstacles", + [ + ("cyl_pos_x", []), + ("cyl_pos_y", []), + ("cyl_rads", []), + ("cyl_heights", []), + ], + ) self.obstacles = { "cyl_pos_x": p_obstacles[0].get_parameter_value().double_array_value, "cyl_pos_y": p_obstacles[1].get_parameter_value().double_array_value, @@ -122,34 +129,40 @@ def __init__(self): self.B_ideal = self.declare_parameter("B_ideal", False).get_parameter_value().bool_value # simulation params - p_sim = self.declare_parameters("", [ - ("sim_dt", 0.01), # update period in [s] - ("discretization", "Euler"), # discretization scheme from {"Euler", "RungeKutta"} - ("x_0", [0.6, 2., 0., 0., 0., 0.]), # initial state - ("B_sim_contacts", True), # if True, simulates contacts - ]) + p_sim = self.declare_parameters( + "", + [ + ("sim_dt", 0.01), # update period in [s] + ("discretization", "Euler"), # discretization scheme from {"Euler", "RungeKutta"} + ("x_0", [0.6, 2.0, 0.0, 0.0, 0.0, 0.0]), # initial state + ("B_sim_contacts", True), # if True, simulates contacts + ], + ) self.SIM_DT = p_sim[0].get_parameter_value().double_value self.DISCRETIZATION = p_sim[1].get_parameter_value().string_value self.x_0 = np.array(p_sim[2].get_parameter_value().double_array_value) self.B_sim_contacts = p_sim[3].get_parameter_value().bool_value # commands, in world frame - self.F_cmd_body = np.zeros(2) # (x,y) - self.theta_dot_cmd = 0. + self.F_cmd_body = np.zeros(2) # (x,y) + self.theta_dot_cmd = 0.0 # commands self.thrusters_dutycycle_cmd = np.zeros(8) - self.wheel_vel_cmd = 0. + self.wheel_vel_cmd = 0.0 # current states of the freeflyer, in world frame - self.x_cur = self.x_0.copy() # (x, y, theta, v_x, v_y, theta_dot) - assert self.x_cur.shape[0]==self.x_dim, "Wrong size of initial state." + self.x_cur = self.x_0.copy() # (x, y, theta, v_x, v_y, theta_dot) + assert self.x_cur.shape[0] == self.x_dim, "Wrong size of initial state." # subscribers - self.sub_wheel_cmd_vel = self.create_subscription(WheelVelCommand, - "commands/velocity", self.update_wheel_cmd_vel_cb, 10) - self.sub_thrusters_cmd_dutycycle = self.create_subscription(ThrusterCommand, - "commands/duty_cycle", self.update_thrusters_dutycycle_cmd_cb, 10) - self.sub_state_init = self.create_subscription(FreeFlyerStateStamped, - "state_init", self.update_state_init_cb, 10) + self.sub_wheel_cmd_vel = self.create_subscription( + WheelVelCommand, "commands/velocity", self.update_wheel_cmd_vel_cb, 10 + ) + self.sub_thrusters_cmd_dutycycle = self.create_subscription( + ThrusterCommand, "commands/duty_cycle", self.update_thrusters_dutycycle_cmd_cb, 10 + ) + self.sub_state_init = self.create_subscription( + FreeFlyerStateStamped, "state_init", self.update_state_init_cb, 10 + ) # ground truth publishers self.pub_state = self.create_publisher(FreeFlyerStateStamped, f"gt/state", 10) @@ -172,7 +185,7 @@ def sim_loop(self) -> None: self.F_cmd_body = F_bodyFrame # Publish true exerted force on freeflyer, as a mapping of the thrusters command duty cycle - R = self.get_rotmatrix_body_to_world(self.x_cur[2]) + R = self.get_rotmatrix_body_to_world(self.x_cur[2]) F_worldFrame = np.matmul(R, F_bodyFrame) wrench_msg = Wrench2DStamped() wrench_msg.header.stamp = now @@ -186,8 +199,9 @@ def sim_loop(self) -> None: u_cur = np.array([self.F_cmd_body[0], self.F_cmd_body[1], M]) # ToDo add more precise simulation (smaller dt) - x_next = self.compute_dynamics_dt(self.x_cur, u_cur, self.SIM_DT, - discretization=self.DISCRETIZATION) + x_next = self.compute_dynamics_dt( + self.x_cur, u_cur, self.SIM_DT, discretization=self.DISCRETIZATION + ) # simulate contacts if self.B_sim_contacts: @@ -223,17 +237,19 @@ def update_wheel_cmd_vel_cb(self, msg: WheelVelCommand) -> None: def update_thrusters_dutycycle_cmd_cb(self, msg: ThrusterCommand) -> None: # Saturate controls so within [0,1] (in %) - self.thrusters_dutycycle_cmd = np.clip(msg.duty_cycle, 0., 1.) + self.thrusters_dutycycle_cmd = np.clip(msg.duty_cycle, 0.0, 1.0) def update_state_init_cb(self, msg: FreeFlyerStateStamped) -> None: - self.x_cur = np.array([ - msg.state.pose.x, - msg.state.pose.y, - msg.state.pose.theta, - msg.state.twist.vx, - msg.state.twist.vy, - msg.state.twist.wz, - ]) + self.x_cur = np.array( + [ + msg.state.pose.x, + msg.state.pose.y, + msg.state.pose.theta, + msg.state.twist.vx, + msg.state.twist.vy, + msg.state.twist.wz, + ] + ) def thrusters_dutycycle_to_body_wrench(self, thrusters_dutycycles_vec): # Thrusters Configuration @@ -254,11 +270,12 @@ def thrusters_dutycycle_to_body_wrench(self, thrusters_dutycycles_vec): u_dc = thrusters_dutycycles_vec - F_x = Fmax * ( (u_dc[2]+u_dc[5]) - (u_dc[1]+u_dc[6]) ) - F_y = Fmax * ( (u_dc[4]+u_dc[7]) - (u_dc[0]+u_dc[3]) ) + F_x = Fmax * ((u_dc[2] + u_dc[5]) - (u_dc[1] + u_dc[6])) + F_y = Fmax * ((u_dc[4] + u_dc[7]) - (u_dc[0] + u_dc[3])) - M = (dist*Fmax) * ( (u_dc[1]+u_dc[3]+u_dc[5]+u_dc[7]) - - (u_dc[0]+u_dc[2]+u_dc[4]+u_dc[6]) ) + M = (dist * Fmax) * ( + (u_dc[1] + u_dc[3] + u_dc[5] + u_dc[7]) - (u_dc[0] + u_dc[2] + u_dc[4] + u_dc[6]) + ) return np.array([F_x, F_y]), M @@ -271,9 +288,11 @@ def actuators_mapping(self, u_cmd): gamma_max = self.p.actuators["gamma_max"] # returns the true wrench u from a commanded wrench u - u_max = np.array([F_max, F_max, M_max])*np.ones_like(u_cmd) + u_max = np.array([F_max, F_max, M_max]) * np.ones_like(u_cmd) u_min = min_inp_percent * u_max - d_max = max_inp_percent * u_max # inflexion point where input saturation occurs (due to PWMs overlapping) + d_max = ( + max_inp_percent * u_max + ) # inflexion point where input saturation occurs (due to PWMs overlapping) # ideal u = u_cmd @@ -284,21 +303,24 @@ def np_sigmoid_sharp(x, k): # smooth approximation to np.heaviside (step function) # alternatively, sharper variant of sigmoid. # k=1 corresponds to sigmoid, and k=\infty is step function. - return 1./(1. + np.exp(-k*x)) + return 1.0 / (1.0 + np.exp(-k * x)) - u = np_sigmoid_sharp(u-u_min, gamma_min)*u + np_sigmoid_sharp(-u-u_min, gamma_min)*u + u = np_sigmoid_sharp(u - u_min, gamma_min) * u + np_sigmoid_sharp(-u - u_min, gamma_min) * u # increase close to 100% - u = u + np_sigmoid_sharp(u-d_max, gamma_max)*u + np_sigmoid_sharp(-u-d_max, gamma_max)*u + u = ( + u + + np_sigmoid_sharp(u - d_max, gamma_max) * u + + np_sigmoid_sharp(-u - d_max, gamma_max) * u + ) # saturate - u = np.maximum(-u_max, - np.minimum(u_max, u)) + u = np.maximum(-u_max, np.minimum(u_max, u)) return u def get_rotmatrix_body_to_world(self, theta): - R = np.array([[np.cos(theta),-np.sin(theta)],[np.sin(theta),np.cos(theta)]]) + R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) return R def f_dynamics_continuous_time(self, x, u): @@ -315,25 +337,24 @@ def f_dynamics_continuous_time(self, x, u): if not self.B_ideal: u = self.actuators_mapping(u) - # Extract state and control r, theta, v, thetadot = x[0:2], x[2], x[3:5], x[5] - F, M = u[0:2], u[2] + F, M = u[0:2], u[2] # Rotation matrix R = self.get_rotmatrix_body_to_world(theta) f = np.zeros(6) f[0:2] = v - f[2] = thetadot - thetaddot = (M - F[1]*p0[0] + F[0]*p0[1]) / J - f[3:5] = np.matmul(R, (F/m - - (thetaddot*np.array([-p0[1],p0[0]]) - thetadot**2*p0) ) ) - f[5] = thetaddot + f[2] = thetadot + thetaddot = (M - F[1] * p0[0] + F[0] * p0[1]) / J + f[3:5] = np.matmul( + R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0)) + ) + f[5] = thetaddot # add constant force due to table tilt - f[3:5] = f[3:5] + F_tilt/m - + f[3:5] = f[3:5] + F_tilt / m return f @@ -345,14 +366,14 @@ def compute_dynamics_dt(self, x_k, u_k, dt, discretization="Euler"): elif discretization == "RungeKutta": k1 = self.f_dynamics_continuous_time(x_k, u_k) - x2 = x_k + 0.5*dt*k1 - k2 = self.f_dynamics_continuous_time(x2, u_k) - x3 = x_k + 0.5*dt*k2 - k3 = self.f_dynamics_continuous_time(x3, u_k) - x4 = x_k + dt*k3 - k4 = self.f_dynamics_continuous_time(x4, u_k) + x2 = x_k + 0.5 * dt * k1 + k2 = self.f_dynamics_continuous_time(x2, u_k) + x3 = x_k + 0.5 * dt * k2 + k3 = self.f_dynamics_continuous_time(x3, u_k) + x4 = x_k + dt * k3 + k4 = self.f_dynamics_continuous_time(x4, u_k) - x_next = x_k + (1./6.)*dt*(k1 + 2.*k2 + 2.*k3 + k4) + x_next = x_k + (1.0 / 6.0) * dt * (k1 + 2.0 * k2 + 2.0 * k3 + k4) else: print("[FreeFlyerSimulator::compute_dynamics_dt]: Unknown Discretization Scheme.") @@ -367,5 +388,5 @@ def main(): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/ff_sim/launch/single.launch.py b/ff_sim/launch/single.launch.py index 53edc62..df3d9c7 100644 --- a/ff_sim/launch/single.launch.py +++ b/ff_sim/launch/single.launch.py @@ -26,21 +26,24 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node + def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") - return LaunchDescription([ - DeclareLaunchArgument("robot_name", default_value="robot"), - Node( - package="ff_params", - executable="robot_params_node", - name="robot_params_node", - namespace=robot_name, - ), - Node( - package="ff_sim", - executable="simulator_node", - name="simulator_node", - namespace=robot_name, - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("robot_name", default_value="robot"), + Node( + package="ff_params", + executable="robot_params_node", + name="robot_params_node", + namespace=robot_name, + ), + Node( + package="ff_sim", + executable="simulator_node", + name="simulator_node", + namespace=robot_name, + ), + ] + ) diff --git a/ff_sim/setup.py b/ff_sim/setup.py index 08150c2..8c8f284 100644 --- a/ff_sim/setup.py +++ b/ff_sim/setup.py @@ -1,28 +1,25 @@ from glob import glob from setuptools import setup -package_name = 'ff_sim' +package_name = "ff_sim" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + "/launch", glob('launch/*launch.[pxy][yma]*')), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*launch.[pxy][yma]*")), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='alvin', - maintainer_email='alvinsunyixiao@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="alvin", + maintainer_email="alvinsunyixiao@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'simulator_node = ff_sim.simulator_node:main' - ], + "console_scripts": ["simulator_node = ff_sim.simulator_node:main"], }, ) diff --git a/ff_sim/test/test_copyright.py b/ff_sim/test/test_copyright.py index 590189a..defd60e 100644 --- a/ff_sim/test/test_copyright.py +++ b/ff_sim/test/test_copyright.py @@ -20,5 +20,5 @@ @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/ff_viz/launch/ff_viz.launch.py b/ff_viz/launch/ff_viz.launch.py index 8d50ec3..4dcf8d9 100644 --- a/ff_viz/launch/ff_viz.launch.py +++ b/ff_viz/launch/ff_viz.launch.py @@ -32,37 +32,40 @@ def generate_launch_description(): - urdf = os.path.join(get_package_share_directory('ff_viz'), 'model', 'ff.urdf.xacro') - robot_name = LaunchConfiguration('robot_name') + urdf = os.path.join(get_package_share_directory("ff_viz"), "model", "ff.urdf.xacro") + robot_name = LaunchConfiguration("robot_name") - return LaunchDescription([ - DeclareLaunchArgument('robot_name', default_value='robot'), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - namespace=robot_name, - parameters=[{ - 'robot_description': ParameterValue( - Command(['xacro ', urdf, ' robot_name:=', robot_name]), - value_type=str - ) - }], - ), - Node( - package='ff_viz', - executable='renderer_node', - name='renderer_node', - parameters=[{'robot_names': [robot_name]}] - ), - Node( - package='rviz2', - executable='rviz2', - name='rviz2', - namespace=robot_name, - arguments=[ - '-d', - os.path.join(get_package_share_directory('ff_viz'), 'rviz', 'default.rviz'), - ], - ) - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("robot_name", default_value="robot"), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=robot_name, + parameters=[ + { + "robot_description": ParameterValue( + Command(["xacro ", urdf, " robot_name:=", robot_name]), value_type=str + ) + } + ], + ), + Node( + package="ff_viz", + executable="renderer_node", + name="renderer_node", + parameters=[{"robot_names": [robot_name]}], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + namespace=robot_name, + arguments=[ + "-d", + os.path.join(get_package_share_directory("ff_viz"), "rviz", "default.rviz"), + ], + ), + ] + ) diff --git a/freeflyer/CMakeLists.txt b/freeflyer/CMakeLists.txt index 1b2608a..d1be08a 100644 --- a/freeflyer/CMakeLists.txt +++ b/freeflyer/CMakeLists.txt @@ -18,11 +18,13 @@ install(DIRECTORY launch if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + find_package(ament_cmake_pytest REQUIRED) # disable Python style checkers set(AMENT_LINT_AUTO_EXCLUDE ament_cmake_flake8 ament_cmake_pep257) ament_lint_auto_find_test_dependencies() + ament_add_pytest_test(black_formatting test/black_formatting.py) add_launch_test(test/pd_ctrl_launch_test.py) endif() diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index 3d90ed9..f466a3e 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -29,31 +29,37 @@ def generate_launch_description(): - robot_name = LaunchConfiguration('robot_name') + 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='key_teleop_node', - name='key_teleop_node', - namespace=robot_name, - prefix='gnome-terminal --', - ), - ]) + 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="key_teleop_node", + name="key_teleop_node", + namespace=robot_name, + prefix="gnome-terminal --", + ), + ] + ) diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 5da4444..af35204 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -29,30 +29,36 @@ def generate_launch_description(): - robot_name = LaunchConfiguration('robot_name') + 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, - ), - ]) + 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, + ), + ] + ) diff --git a/freeflyer/test/black_formatting.py b/freeflyer/test/black_formatting.py new file mode 100644 index 0000000..8cd54d4 --- /dev/null +++ b/freeflyer/test/black_formatting.py @@ -0,0 +1,45 @@ +#!/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 unittest + +import os +import sys +from subprocess import check_call, CalledProcessError +from pathlib import Path +import contextlib + + +class TestPythonFormatting(unittest.TestCase): + def test_check_python_formatting(self): + root_path = Path(__file__).absolute().parents[2] + try: + check_call(["black", "--check", str(root_path)]) + failed = False + except CalledProcessError: + failed = True + if failed: + msg = "Python code is not formatted. Run `black .` in `freeflyer2` root to format." + self.fail(msg) diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py index 6f4ac58..6ff8f03 100644 --- a/freeflyer/test/pd_ctrl_launch_test.py +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -36,34 +36,37 @@ import rclpy -ROBOT_NAME = 'freeflyer' +ROBOT_NAME = "freeflyer" def generate_test_description(): sim_launch = IncludeLaunchDescription( - PathJoinSubstitution([ - FindPackageShare('ff_sim'), - 'launch', - 'single.launch.py', - ]), - launch_arguments={'robot_name': ROBOT_NAME}.items(), + PathJoinSubstitution( + [ + FindPackageShare("ff_sim"), + "launch", + "single.launch.py", + ] + ), + launch_arguments={"robot_name": ROBOT_NAME}.items(), ) pd_ctrl_node = Node( - package='ff_control', - executable='pd_ctrl_node', - name='pd_ctrl_node', + package="ff_control", + executable="pd_ctrl_node", + name="pd_ctrl_node", namespace=ROBOT_NAME, ) - return LaunchDescription([ - sim_launch, - pd_ctrl_node, - ReadyToTest(), - ]), {'sim_launch': sim_launch, 'pd_ctrl_node': pd_ctrl_node} + return LaunchDescription( + [ + sim_launch, + pd_ctrl_node, + ReadyToTest(), + ] + ), {"sim_launch": sim_launch, "pd_ctrl_node": pd_ctrl_node} class TestPDControlNode(unittest.TestCase): - @classmethod def setUpClass(cls): rclpy.init() @@ -73,7 +76,7 @@ def tearDownClass(cls): rclpy.shutdown() def setUp(self): - self.node = rclpy.create_node('test_pd_ctrl_node') + self.node = rclpy.create_node("test_pd_ctrl_node") def tearDown(self): self.node.destroy_node() @@ -87,46 +90,48 @@ def state_callback(msg): sub = self.node.create_subscription( FreeFlyerStateStamped, - f'/{ROBOT_NAME}/gt/state', + f"/{ROBOT_NAME}/gt/state", state_callback, 10, ) pub = self.node.create_publisher( FreeFlyerStateStamped, - f'/{ROBOT_NAME}/ctrl/state', + f"/{ROBOT_NAME}/ctrl/state", 10, ) try: # wait for nodes to start up (with 5 seconds timeout) - end_time = time.time() + 5. + end_time = time.time() + 5.0 node_flag = False while time.time() < end_time and not node_flag: - node_flag = 'pd_ctrl_node' in self.node.get_node_names() and \ - 'simulator_node' in self.node.get_node_names() + node_flag = ( + "pd_ctrl_node" in self.node.get_node_names() + and "simulator_node" in self.node.get_node_names() + ) time.sleep(0.1) - assert node_flag, 'pd_ctrl_node or simualtor_node launch failure' + assert node_flag, "pd_ctrl_node or simualtor_node launch failure" # wait for node to initialize - time.sleep(3.) + time.sleep(3.0) # publish target state target_state = FreeFlyerStateStamped() target_state.header.stamp = self.node.get_clock().now().to_msg() - target_state.state.pose.x = 1. - target_state.state.pose.y = 1. - target_state.state.pose.theta = 1. + target_state.state.pose.x = 1.0 + target_state.state.pose.y = 1.0 + target_state.state.pose.theta = 1.0 pub.publish(target_state) # wait for 15 seconds and check results - end_time = time.time() + 15. + end_time = time.time() + 15.0 while time.time() < end_time: rclpy.spin_once(self.node, timeout_sec=0.1) # current state should be close to the target state - self.assertAlmostEquals(current_state.state.pose.x, 1., delta=1e-1) - self.assertAlmostEquals(current_state.state.pose.y, 1., delta=1e-1) - self.assertAlmostEquals(current_state.state.pose.theta, 1., delta=1e-1) + self.assertAlmostEquals(current_state.state.pose.x, 1.0, delta=1e-1) + self.assertAlmostEquals(current_state.state.pose.y, 1.0, delta=1e-1) + self.assertAlmostEquals(current_state.state.pose.theta, 1.0, delta=1e-1) finally: self.node.destroy_subscription(sub) diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..044a852 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,17 @@ +[tool.black] +line-length = 100 +target-version = ['py310'] +include = '\.pyi?$' +exclude = ''' +/( + \.git + | \.hg + | \.mypy_cache + | \.tox + | \.venv + | _build + | buck-out + | build + | dist +)/ +''' From 085713c7977cb8ebd0354d7e4e024545a45b71d3 Mon Sep 17 00:00:00 2001 From: Robert Dyro Date: Thu, 18 May 2023 11:14:25 -0700 Subject: [PATCH 46/56] adding README comment about black & installing black in CI images --- .github/workflows/foxy.yml | 2 ++ .github/workflows/humble.yml | 2 ++ .github/workflows/rolling.yml | 2 ++ README.md | 22 +++++++++++++++++++++- 4 files changed, 27 insertions(+), 1 deletion(-) diff --git a/.github/workflows/foxy.yml b/.github/workflows/foxy.yml index ae9639b..7ecbe18 100644 --- a/.github/workflows/foxy.yml +++ b/.github/workflows/foxy.yml @@ -14,6 +14,8 @@ jobs: steps: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 + - name: Install dependencies + run: pip install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index 6a19c3b..b1ae4a3 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -14,6 +14,8 @@ jobs: steps: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 + - name: Install dependencies + run: pip install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 7b45ce2..deb02c4 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -14,6 +14,8 @@ jobs: steps: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 + - name: Install dependencies + run: pip install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: diff --git a/README.md b/README.md index c5cde51..7df50a6 100644 --- a/README.md +++ b/README.md @@ -32,6 +32,11 @@ The CI will build and run all registered tests. PR can be merged only when all s pass. [ROS2 code styles](https://docs.ros.org/en/humble/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html) are enforced for both C++ and Python. Here are some tips for fixing code style issues for a PR. +To manually run all tests run +```sh +$ colcon build && colcon test && colcon test-result --verbose +``` + #### Copyright Every source file (e.g. `.cpp`, `.hpp`, `.py`) including launch files should have a copy of the license at the very top. See any source files for an example. @@ -43,7 +48,7 @@ the license at the very top. See any source files for an example. #### Python Code Style For the first time, you will need to run the following command to install some dev packages ```sh -sudo apt install -y \ +$ sudo apt install -y \ python3-flake8-docstrings \ python3-flake8-blind-except \ python3-flake8-builtins \ @@ -58,6 +63,21 @@ sudo apt install -y \ 1. Run `ament_flake8` to check for code style violations and fix them manually. 2. Run `ament_pep257` to check for docstring style violations and fix them manually. +**We are now enforcing Python source code formatting with the `black` +formatter.** Please install `black` with `pip install black`. + +Importantly +- to format all Python code with black automatically run +```sh +$ cd freeflyer2 +$ black . +``` +- to check formatting without changing the files run +```sh +$ cd freeflyer2 +$ black --check . +``` + #### Disable Code Style Test If you hate the ROS2 conventions so bad, you can disable specific code style test by adding the following line before `ament_lint_auto_find_test_dependencies()` in `CMakeLists.txt`. From 6945067da04a7f124bbb860421250b89c7fc1331 Mon Sep 17 00:00:00 2001 From: Robert Dyro Date: Thu, 18 May 2023 11:16:27 -0700 Subject: [PATCH 47/56] fixing pip not found in CI --- .github/workflows/foxy.yml | 2 +- .github/workflows/humble.yml | 2 +- .github/workflows/rolling.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/foxy.yml b/.github/workflows/foxy.yml index 7ecbe18..7ad8ed7 100644 --- a/.github/workflows/foxy.yml +++ b/.github/workflows/foxy.yml @@ -15,7 +15,7 @@ jobs: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 - name: Install dependencies - run: pip install black + run: pip3 install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index b1ae4a3..d838326 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -15,7 +15,7 @@ jobs: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 - name: Install dependencies - run: pip install black + run: pip3 install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index deb02c4..079c6a4 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -15,7 +15,7 @@ jobs: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 - name: Install dependencies - run: pip install black + run: pip3 install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: From 4e9ff74ea4acacf6311c032cad834e133891c9fd Mon Sep 17 00:00:00 2001 From: Robert Dyro Date: Thu, 18 May 2023 11:19:07 -0700 Subject: [PATCH 48/56] another CI fix --- .github/workflows/foxy.yml | 4 ++-- .github/workflows/humble.yml | 4 ++-- .github/workflows/rolling.yml | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/foxy.yml b/.github/workflows/foxy.yml index 7ad8ed7..67ae9ee 100644 --- a/.github/workflows/foxy.yml +++ b/.github/workflows/foxy.yml @@ -14,12 +14,12 @@ jobs: steps: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 - - name: Install dependencies - run: pip3 install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: required-ros-distributions: foxy + - name: Install dependencies + run: pip install black - name: ROS2 build and test uses: ros-tooling/action-ros-ci@v0.2 with: diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index d838326..8a829e4 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -14,12 +14,12 @@ jobs: steps: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 - - name: Install dependencies - run: pip3 install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: required-ros-distributions: humble + - name: Install dependencies + run: pip install black - name: ROS2 build and test uses: ros-tooling/action-ros-ci@v0.2 with: diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 079c6a4..1267b6b 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -14,12 +14,12 @@ jobs: steps: - name: cancle preivious run uses: styfle/cancel-workflow-action@0.11.0 - - name: Install dependencies - run: pip3 install black - name: setup ROS2 uses: ros-tooling/setup-ros@v0.5 with: required-ros-distributions: rolling + - name: Install dependencies + run: pip install black - name: ROS2 build and test uses: ros-tooling/action-ros-ci@v0.2 with: From 1c2bd24ebcb8b5ea3e515aee339c8d2a38545cb8 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 18 May 2023 12:11:26 -0700 Subject: [PATCH 49/56] update readme --- README.md | 26 +++++--------------------- 1 file changed, 5 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 7df50a6..83f35fb 100644 --- a/README.md +++ b/README.md @@ -30,9 +30,11 @@ source ~/ff_ws/install/local_setup.bash ## Development Guide The CI will build and run all registered tests. PR can be merged only when all status checks pass. [ROS2 code styles](https://docs.ros.org/en/humble/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html) -are enforced for both C++ and Python. Here are some tips for fixing code style issues for a PR. +are enforced for C++. Python code style is enforced through `black`. +Here are some tips for fixing code style issues for a PR. -To manually run all tests run +Run the following command to manually run all tests and see detailed test results including +format violation ```sh $ colcon build && colcon test && colcon test-result --verbose ``` @@ -46,25 +48,7 @@ the license at the very top. See any source files for an example. 2. Run `ament_cpplint` to check for style violations and fix them manually. #### Python Code Style -For the first time, you will need to run the following command to install some dev packages -```sh -$ sudo apt install -y \ - python3-flake8-docstrings \ - python3-flake8-blind-except \ - python3-flake8-builtins \ - python3-flake8-class-newline \ - python3-flake8-comprehensions \ - python3-flake8-deprecated \ - python3-flake8-import-order \ - python3-flake8-quotes \ - python3-pytest-repeat \ - python3-pytest-rerunfailures -``` -1. Run `ament_flake8` to check for code style violations and fix them manually. -2. Run `ament_pep257` to check for docstring style violations and fix them manually. - -**We are now enforcing Python source code formatting with the `black` -formatter.** Please install `black` with `pip install black`. +Please install `black` with `pip install black` for the first time. Importantly - to format all Python code with black automatically run From bda2e9a65f32328d6ee6b42a9634061bbdab3fd9 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Thu, 18 May 2023 15:32:26 -0700 Subject: [PATCH 50/56] fix test errors --- ff_drivers/CMakeLists.txt | 9 +- ff_drivers/include/ff_drivers/pwm.hpp | 79 +++++++--- ff_drivers/launch/hardware_bringup.launch.py | 80 ++++++---- .../launch/test_all_thrusters.launch.py | 56 +++++-- ff_drivers/src/pwm.cpp | 147 +++++++++++++----- ff_drivers/src/tests/test_all_thrusters.cpp | 40 ++++- ff_drivers/src/tests/test_single.cpp | 42 ++++- ff_drivers/src/thruster_node.cpp | 45 +++++- freeflyer/launch/hardware_pd.launch.py | 90 +++++++---- freeflyer/launch/mocap.launch.py | 54 +++++-- 10 files changed, 462 insertions(+), 180 deletions(-) diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt index 4d67542..734adbf 100644 --- a/ff_drivers/CMakeLists.txt +++ b/ff_drivers/CMakeLists.txt @@ -40,13 +40,8 @@ install(DIRECTORY launch if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + # disable Python style checkers + set(AMENT_LINT_AUTO_EXCLUDE ament_cmake_flake8 ament_cmake_pep257) ament_lint_auto_find_test_dependencies() endif() diff --git a/ff_drivers/include/ff_drivers/pwm.hpp b/ff_drivers/include/ff_drivers/pwm.hpp index 8c40cd9..256f8b5 100644 --- a/ff_drivers/include/ff_drivers/pwm.hpp +++ b/ff_drivers/include/ff_drivers/pwm.hpp @@ -1,3 +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. + + #pragma once #include @@ -8,10 +31,12 @@ #include -namespace ff { +namespace ff +{ -class PWM { - public: +class PWM +{ +public: virtual void High() {} virtual void Low() {} virtual bool IsSoft() const = 0; @@ -19,12 +44,12 @@ class PWM { virtual void Disable(); virtual void SetPolarityNormal(); virtual void SetPolarityInverse(); - virtual void SetPeriod(const std::chrono::duration& period); - virtual void SetDutyCycle(const std::chrono::duration& duty_cycle); + virtual void SetPeriod(const std::chrono::duration & period); + virtual void SetDutyCycle(const std::chrono::duration & duty_cycle); void SetDutyCyclePercent(double percent); void Initialize(); - private: +private: bool enabled_ = false; bool polarity_inversed_ = false; std::chrono::duration period_ = std::chrono::seconds(0); @@ -38,27 +63,29 @@ class PWM { * * @see https://wiki.odroid.com/odroid-n2l/application_note/gpio/pwm#tab__odroid-n21 for pin maps */ -class HardPWM : public PWM { - public: - enum PinDef { +class HardPWM : public PWM +{ +public: + enum PinDef + { PWM_C = 0, PWM_D, PWM_E, PWM_F, }; - HardPWM(const PinDef& pwm); + explicit HardPWM(const PinDef & pwm); ~HardPWM(); - bool IsSoft() const override { return false; } + bool IsSoft() const override {return false;} void Enable() override; void Disable() override; void SetPolarityNormal() override; void SetPolarityInverse() override; - void SetPeriod(const std::chrono::duration& period) override; - void SetDutyCycle(const std::chrono::duration& duty_cycle) override; + void SetPeriod(const std::chrono::duration & period) override; + void SetDutyCycle(const std::chrono::duration & duty_cycle) override; - private: +private: std::string ChipBasePath() const; std::string PWMBasePath() const; @@ -75,18 +102,19 @@ class HardPWM : public PWM { * @see https://wiki.odroid.com/odroid-n2l/application_note/gpio/enhancement_40pins#tab__odroid-n2 * for pin numbering */ -class SoftPWM : public PWM { - public: - SoftPWM(int pin); +class SoftPWM : public PWM +{ +public: + explicit SoftPWM(int pin); ~SoftPWM(); - bool IsSoft() const override { return true; } + bool IsSoft() const override {return true;} void High() override; void Low() override; void SetPolarityNormal() override; void SetPolarityInverse() override; - private: +private: int pin_; std::ofstream f_value_; std::ofstream f_active_low_; @@ -94,19 +122,20 @@ class SoftPWM : public PWM { std::string GPIOBasePath() const; }; -class PWMManager : public rclcpp::Node { - public: - PWMManager(const std::string& node_name); +class PWMManager : public rclcpp::Node +{ +public: + explicit PWMManager(const std::string & node_name); void AddSoftPWM(int pin); - void AddHardPWM(const HardPWM::PinDef& pin); + void AddHardPWM(const HardPWM::PinDef & pin); void EnableAll(); void DisableAll(); - void SetPeriodAll(const std::chrono::duration& period); + void SetPeriodAll(const std::chrono::duration & period); void SetDutyCycle(size_t idx, double duty_cycle_percent); - private: +private: // software PWM control std::chrono::duration soft_period_; rclcpp::TimerBase::SharedPtr period_timer_; diff --git a/ff_drivers/launch/hardware_bringup.launch.py b/ff_drivers/launch/hardware_bringup.launch.py index bcd1f62..48caaa0 100644 --- a/ff_drivers/launch/hardware_bringup.launch.py +++ b/ff_drivers/launch/hardware_bringup.launch.py @@ -1,34 +1,62 @@ +# 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 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node + def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") - return LaunchDescription([ - DeclareLaunchArgument("robot_name", default_value="robot"), - Node( - package="ff_params", - executable="robot_params_node", - name="robot_params_node", - namespace=robot_name, - ), - Node( - package="ff_drivers", - executable="thruster_node", - name="thruster_node", - namespace=robot_name, - ), - Node( - package="vrpn_mocap", - executable="client_node", - name="vrpn_client_node", - namespace=PathJoinSubstitution([robot_name, "mocap"]), - # ASL optitrack IP and port - parameters=[{ - "server": "192.168.1.8", - "port": 3883, - }], - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("robot_name", default_value="robot"), + Node( + package="ff_params", + executable="robot_params_node", + name="robot_params_node", + namespace=robot_name, + ), + Node( + package="ff_drivers", + executable="thruster_node", + name="thruster_node", + namespace=robot_name, + ), + Node( + package="vrpn_mocap", + executable="client_node", + name="vrpn_client_node", + namespace=PathJoinSubstitution([robot_name, "mocap"]), + # ASL optitrack IP and port + parameters=[ + { + "server": "192.168.1.8", + "port": 3883, + } + ], + ), + ] + ) diff --git a/ff_drivers/launch/test_all_thrusters.launch.py b/ff_drivers/launch/test_all_thrusters.launch.py index 68c179d..eb6879a 100644 --- a/ff_drivers/launch/test_all_thrusters.launch.py +++ b/ff_drivers/launch/test_all_thrusters.launch.py @@ -1,23 +1,49 @@ +# 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 from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node + def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") - return LaunchDescription([ - DeclareLaunchArgument("robot_name", default_value="robot"), - Node( - package="ff_drivers", - executable="thruster_node", - name="thruster_node", - namespace=robot_name, - ), - Node( - package="ff_drivers", - executable="test_all_thrusters", - name="test_all_thrusters", - namespace=robot_name, - ) - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("robot_name", default_value="robot"), + Node( + package="ff_drivers", + executable="thruster_node", + name="thruster_node", + namespace=robot_name, + ), + Node( + package="ff_drivers", + executable="test_all_thrusters", + name="test_all_thrusters", + namespace=robot_name, + ), + ] + ) diff --git a/ff_drivers/src/pwm.cpp b/ff_drivers/src/pwm.cpp index c92288f..938e508 100644 --- a/ff_drivers/src/pwm.cpp +++ b/ff_drivers/src/pwm.cpp @@ -1,46 +1,80 @@ +// 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. + + #include "ff_drivers/pwm.hpp" -namespace ff { +namespace ff +{ -void PWM::Enable() { +void PWM::Enable() +{ enabled_ = true; } -void PWM::Disable() { +void PWM::Disable() +{ enabled_ = false; } -void PWM::SetPolarityNormal() { +void PWM::SetPolarityNormal() +{ polarity_inversed_ = false; } -void PWM::SetPolarityInverse() { +void PWM::SetPolarityInverse() +{ polarity_inversed_ = true; } -void PWM::SetPeriod(const std::chrono::duration& period) { +void PWM::SetPeriod(const std::chrono::duration & period) +{ period_ = period; } -void PWM::SetDutyCycle(const std::chrono::duration& duty_cycle) { +void PWM::SetDutyCycle(const std::chrono::duration & duty_cycle) +{ duty_cycle_ = duty_cycle; } -void PWM::SetDutyCyclePercent(double percent) { +void PWM::SetDutyCyclePercent(double percent) +{ SetDutyCycle(percent * period_); } -void PWM::Initialize() { +void PWM::Initialize() +{ Disable(); SetPolarityNormal(); SetPeriod(period_); SetDutyCycle(duty_cycle_); } -constexpr int pwmchip[] = { 4, 4, 8, 8 }; -constexpr int pwmpin[] = { 0, 1, 0, 1 }; +constexpr int pwmchip[] = {4, 4, 8, 8}; +constexpr int pwmpin[] = {0, 1, 0, 1}; -HardPWM::HardPWM(const HardPWM::PinDef& pwm) : pwm_(pwm) { +HardPWM::HardPWM(const HardPWM::PinDef & pwm) +: pwm_(pwm) +{ // request device { std::ofstream f_export(ChipBasePath() + "export"); @@ -55,7 +89,8 @@ HardPWM::HardPWM(const HardPWM::PinDef& pwm) : pwm_(pwm) { Initialize(); } -HardPWM::~HardPWM() { +HardPWM::~HardPWM() +{ Disable(); // close files @@ -69,51 +104,61 @@ HardPWM::~HardPWM() { f_unexport << pwmpin[pwm_]; } -void HardPWM::Enable() { +void HardPWM::Enable() +{ f_enable_ << 1; f_enable_.flush(); PWM::Enable(); } -void HardPWM::Disable() { +void HardPWM::Disable() +{ f_enable_ << 0; f_enable_.flush(); PWM::Disable(); } -void HardPWM::SetPolarityNormal() { +void HardPWM::SetPolarityNormal() +{ f_polarity_ << "normal"; f_polarity_.flush(); PWM::SetPolarityNormal(); } -void HardPWM::SetPolarityInverse() { +void HardPWM::SetPolarityInverse() +{ f_polarity_ << "inversed"; f_polarity_.flush(); PWM::SetPolarityInverse(); } -void HardPWM::SetPeriod(const std::chrono::duration& period) { +void HardPWM::SetPeriod(const std::chrono::duration & period) +{ f_period_ << std::chrono::duration_cast(period).count(); f_period_.flush(); PWM::SetPeriod(period); } -void HardPWM::SetDutyCycle(const std::chrono::duration& duty_cycle) { +void HardPWM::SetDutyCycle(const std::chrono::duration & duty_cycle) +{ f_duty_cycle_ << std::chrono::duration_cast(duty_cycle).count(); f_duty_cycle_.flush(); PWM::SetDutyCycle(duty_cycle); } -std::string HardPWM::ChipBasePath() const { +std::string HardPWM::ChipBasePath() const +{ return "/sys/class/pwm/pwmchip" + std::to_string(pwmchip[pwm_]) + "/"; } -std::string HardPWM::PWMBasePath() const { +std::string HardPWM::PWMBasePath() const +{ return ChipBasePath() + "pwm" + std::to_string(pwmpin[pwm_]) + "/"; } -SoftPWM::SoftPWM(int pin) : pin_(pin) { +SoftPWM::SoftPWM(int pin) +: pin_(pin) +{ // request gpio device { std::ofstream f_export("/sys/class/gpio/export"); @@ -132,7 +177,8 @@ SoftPWM::SoftPWM(int pin) : pin_(pin) { Initialize(); } -SoftPWM::~SoftPWM() { +SoftPWM::~SoftPWM() +{ Disable(); Low(); @@ -145,59 +191,70 @@ SoftPWM::~SoftPWM() { f_unexport << pin_; } -void SoftPWM::High() { +void SoftPWM::High() +{ f_value_ << 1; f_value_.flush(); } -void SoftPWM::Low() { +void SoftPWM::Low() +{ f_value_ << 0; f_value_.flush(); } -void SoftPWM::SetPolarityNormal() { +void SoftPWM::SetPolarityNormal() +{ f_active_low_ << 0; f_active_low_.flush(); PWM::SetPolarityNormal(); } -void SoftPWM::SetPolarityInverse() { +void SoftPWM::SetPolarityInverse() +{ f_active_low_ << 1; f_active_low_.flush(); PWM::SetPolarityInverse(); } -std::string SoftPWM::GPIOBasePath() const { +std::string SoftPWM::GPIOBasePath() const +{ return "/sys/class/gpio/gpio" + std::to_string(pin_) + "/"; } -PWMManager::PWMManager(const std::string& node_name) : rclcpp::Node(node_name) {} +PWMManager::PWMManager(const std::string & node_name) +: rclcpp::Node(node_name) {} -void PWMManager::AddSoftPWM(int pin) { +void PWMManager::AddSoftPWM(int pin) +{ pwms_.push_back(std::make_shared(pin)); switch_timers_.push_back(nullptr); } -void PWMManager::AddHardPWM(const HardPWM::PinDef& pin) { +void PWMManager::AddHardPWM(const HardPWM::PinDef & pin) +{ pwms_.push_back(std::make_shared(pin)); switch_timers_.push_back(nullptr); } -void PWMManager::EnableAll() { - for (auto& pwm : pwms_) { +void PWMManager::EnableAll() +{ + for (auto & pwm : pwms_) { pwm->Enable(); } } -void PWMManager::DisableAll() { - for (auto& pwm: pwms_) { +void PWMManager::DisableAll() +{ + for (auto & pwm : pwms_) { pwm->Disable(); } } -void PWMManager::SetPeriodAll(const std::chrono::duration& period) { - for (auto& pwm: pwms_) { +void PWMManager::SetPeriodAll(const std::chrono::duration & period) +{ + for (auto & pwm : pwms_) { pwm->SetPeriod(period); } @@ -205,7 +262,8 @@ void PWMManager::SetPeriodAll(const std::chrono::duration& period) { period_timer_ = this->create_wall_timer(period, std::bind(&PWMManager::PeriodCallback, this)); } -void PWMManager::SetDutyCycle(size_t idx, double duty_cycle_percent) { +void PWMManager::SetDutyCycle(size_t idx, double duty_cycle_percent) +{ if (idx >= pwms_.size()) { RCLCPP_ERROR(this->get_logger(), "PWM idx out of range"); return; @@ -219,7 +277,8 @@ void PWMManager::SetDutyCycle(size_t idx, double duty_cycle_percent) { pwms_[idx]->SetDutyCyclePercent(duty_cycle_percent); } -void PWMManager::PeriodCallback() { +void PWMManager::PeriodCallback() +{ for (size_t i = 0; i < pwms_.size(); ++i) { if (!pwms_[i]) { RCLCPP_ERROR(this->get_logger(), "encountered unitialized PWM pointer"); @@ -228,19 +287,21 @@ void PWMManager::PeriodCallback() { if (pwms_[i]->IsSoft() && pwms_[i]->enabled_) { pwms_[i]->High(); - switch_timers_[i] = this->create_wall_timer(pwms_[i]->duty_cycle_, - [this, i]() { SwitchCallback(i); }); + switch_timers_[i] = this->create_wall_timer( + pwms_[i]->duty_cycle_, + [this, i]() {SwitchCallback(i);}); } } } -void PWMManager::SwitchCallback(size_t idx) { +void PWMManager::SwitchCallback(size_t idx) +{ if (idx >= pwms_.size()) { RCLCPP_ERROR(this->get_logger(), "PWM idx out of range"); return; } - if (!pwms_[idx] || ! switch_timers_[idx]) { + if (!pwms_[idx] || !switch_timers_[idx]) { RCLCPP_ERROR(this->get_logger(), "encountered unitialized pointer"); return; } diff --git a/ff_drivers/src/tests/test_all_thrusters.cpp b/ff_drivers/src/tests/test_all_thrusters.cpp index babee79..afa2e40 100644 --- a/ff_drivers/src/tests/test_all_thrusters.cpp +++ b/ff_drivers/src/tests/test_all_thrusters.cpp @@ -1,3 +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. + + #include #include @@ -8,20 +31,24 @@ using namespace std::chrono_literals; using ff_msgs::msg::ThrusterCommand; -class TestAllThrustersNode : public rclcpp::Node { - public: - TestAllThrustersNode() : rclcpp::Node("test_all_thrusters_node") { +class TestAllThrustersNode : public rclcpp::Node +{ +public: + TestAllThrustersNode() + : rclcpp::Node("test_all_thrusters_node") + { thrust_cmd_pub_ = this->create_publisher("commands/duty_cycle", 10); timer_ = this->create_wall_timer(5s, std::bind(&TestAllThrustersNode::TimerCallback, this)); this->declare_parameter("duty_cycle", .2); } - private: +private: rclcpp::Publisher::SharedPtr thrust_cmd_pub_; rclcpp::TimerBase::SharedPtr timer_; int th_idx_ = 0; - void TimerCallback() { + void TimerCallback() + { double duty_cycle = this->get_parameter("duty_cycle").as_double(); // populate thrust msg @@ -43,7 +70,8 @@ class TestAllThrustersNode : public rclcpp::Node { }; -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/ff_drivers/src/tests/test_single.cpp b/ff_drivers/src/tests/test_single.cpp index 67011e5..ff4ebdb 100644 --- a/ff_drivers/src/tests/test_single.cpp +++ b/ff_drivers/src/tests/test_single.cpp @@ -1,3 +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. + + #include #include @@ -7,9 +30,12 @@ using namespace std::chrono_literals; -class TestSingleNode : public ff::PWMManager { - public: - TestSingleNode() : ff::PWMManager("test_single_node") { +class TestSingleNode : public ff::PWMManager +{ +public: + TestSingleNode() + : ff::PWMManager("test_single_node") + { bool use_hard = this->declare_parameter("use_hard", true); int pin = this->declare_parameter("pin", 473); this->declare_parameter("max_duty_cycle", .2); @@ -25,11 +51,12 @@ class TestSingleNode : public ff::PWMManager { this->SetPeriodAll(100ms); this->EnableAll(); - timer_ = this->create_wall_timer(5s, [this]() { TimerCallback(); }); + timer_ = this->create_wall_timer(5s, [this]() {TimerCallback();}); } - private: - void TimerCallback() { +private: + void TimerCallback() + { double max_duty_cycle = this->get_parameter("max_duty_cycle").as_double(); this->SetDutyCycle(0, cnt_ * max_duty_cycle * .1); @@ -41,7 +68,8 @@ class TestSingleNode : public ff::PWMManager { rclcpp::TimerBase::SharedPtr timer_; }; -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp index 6f692f2..682e726 100644 --- a/ff_drivers/src/thruster_node.cpp +++ b/ff_drivers/src/thruster_node.cpp @@ -1,3 +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. + + #include #include @@ -25,9 +48,12 @@ static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { using namespace std::chrono_literals; using ff_msgs::msg::ThrusterCommand; -class ThrusterNode : public ff::PWMManager { - public: - ThrusterNode() : ff::PWMManager("thruster_driver_node") { +class ThrusterNode : public ff::PWMManager +{ +public: + ThrusterNode() + : ff::PWMManager("thruster_driver_node") + { // add all PWMs for (size_t i = 0; i < NUM_THRUSTERS; ++i) { this->AddSoftPWM(THRUSTER_PINS[i]); @@ -38,23 +64,26 @@ class ThrusterNode : public ff::PWMManager { this->SetPeriodAll(period * 1s); // update period on the fly sub_params_ = std::make_shared(this); - cb_period_ = sub_params_->add_parameter_callback("period", - [this](const rclcpp::Parameter& p) { SetPeriodAll(p.as_double() * 1s); }); + cb_period_ = sub_params_->add_parameter_callback( + "period", + [this](const rclcpp::Parameter & p) {SetPeriodAll(p.as_double() * 1s);}); // start all PWMs this->EnableAll(); // listen to commands - sub_duty_cycle_ = this->create_subscription("commands/duty_cycle", + sub_duty_cycle_ = this->create_subscription( + "commands/duty_cycle", 10, [this](const ThrusterCommand::SharedPtr msg) {DutyCycleCallback(msg);}); } - private: +private: std::shared_ptr sub_params_; std::shared_ptr cb_period_; rclcpp::Subscription::SharedPtr sub_duty_cycle_; - void DutyCycleCallback(const ThrusterCommand::SharedPtr msg) { + void DutyCycleCallback(const ThrusterCommand::SharedPtr msg) + { for (size_t i = 0; i < NUM_THRUSTERS; ++i) { this->SetDutyCycle(i, msg->duty_cycle[i]); } diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index d16459d..4a807d2 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -1,39 +1,69 @@ +# 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_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, - ), - Node( - package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", - namespace=robot_name, - parameters=[{ - "pose_channel": PathJoinSubstitution([ - "mocap", - robot_name, - "pose", - ]), - }], - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("robot_name", default_value="robot"), + # 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, + ), + Node( + package="ff_estimate", + executable="mocap_estimator_node", + name="mocap_estimator_node", + namespace=robot_name, + parameters=[ + { + "pose_channel": PathJoinSubstitution( + [ + "mocap", + robot_name, + "pose", + ] + ), + } + ], + ), + ] + ) diff --git a/freeflyer/launch/mocap.launch.py b/freeflyer/launch/mocap.launch.py index 09e6702..ead9807 100644 --- a/freeflyer/launch/mocap.launch.py +++ b/freeflyer/launch/mocap.launch.py @@ -1,17 +1,45 @@ +# 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_ros.actions import Node + def generate_launch_description(): - return LaunchDescription([ - Node( - package="vrpn_mocap", - executable="client_node", - name="vrpn_client_node", - namespace="mocap", - # ASL optitrack IP and port - parameters=[{ - "server": "192.168.1.8", - "port": 3883, - }], - ), - ]) + return LaunchDescription( + [ + Node( + package="vrpn_mocap", + executable="client_node", + name="vrpn_client_node", + namespace="mocap", + # ASL optitrack IP and port + parameters=[ + { + "server": "192.168.1.8", + "port": 3883, + } + ], + ), + ] + ) From 04e8941f2b790470f9998d9befe1eec92afc26dc Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Mon, 22 May 2023 14:18:24 -0700 Subject: [PATCH 51/56] update pin --- ff_drivers/src/thruster_node.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp index 682e726..d29005b 100644 --- a/ff_drivers/src/thruster_node.cpp +++ b/ff_drivers/src/thruster_node.cpp @@ -34,14 +34,14 @@ // thruster pin connection // @see: https://wiki.odroid.com/odroid-n2l/application_note/gpio/pwm#tab__odroid-n2 static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { - 473, // thruster pin 1 -> odroid pin 7 - 479, // thruster pin 2 -> odroid pin 11 - 480, // thruster pin 3 -> odroid pin 13 - 472, // thruster pin 4 -> odroid pin 32 - 495, // thruster pin 5 -> odroid pin 36 - 476, // thruster pin 6 -> odroid pin 16 - 477, // thruster pin 7 -> odroid pin 18 - 478, // thruster pin 8 -> odroid pin 22 + 476, // thruster pin 1 -> odroid pin 16 + 477, // thruster pin 2 -> odroid pin 18 + 484, // thruster pin 3 -> odroid pin 19 + 485, // thruster pin 4 -> odroid pin 21 + 478, // thruster pin 5 -> odroid pin 22 + 487, // thruster pin 6 -> odroid pin 23 + 486, // thruster pin 7 -> odroid pin 24 + 464, // thruster pin 8 -> odroid pin 26 }; From 2535644f3e32e45f6f59e578577689ce07a7ae6b Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Mon, 22 May 2023 17:05:45 -0700 Subject: [PATCH 52/56] add rviz condition for hardware pd launch --- freeflyer/README.md | 5 +++++ freeflyer/launch/hardware_pd.launch.py | 22 ++++++++++++++-------- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/freeflyer/README.md b/freeflyer/README.md index f6a497f..fd43a51 100644 --- a/freeflyer/README.md +++ b/freeflyer/README.md @@ -22,3 +22,8 @@ the motion capture system. ```sh ros2 launch freeflyer estimation_viz.launch.py ``` + +**PD Waypoint Control in Hardware** +```sh +ros2 launch freeflyer hardware_pd.launch.py rviz:=false # or set rviz:=true to launch RVIZ +``` diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index 4a807d2..00996c5 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -23,6 +23,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -30,18 +31,23 @@ def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") + rviz = LaunchConfiguration("rviz") return LaunchDescription( [ DeclareLaunchArgument("robot_name", default_value="robot"), - # IncludeLaunchDescription( - # PathJoinSubstitution([ - # FindPackageShare("ff_viz"), - # "launch", - # "ff_viz.launch.py", - # ]), - # launch_arguments={"robot_name": robot_name}.items(), - # ), + DeclareLaunchArgument("rviz", default_value="false", + description="set to true to launch rviz", + choices=["true", "false"]) + IncludeLaunchDescription( + PathJoinSubstitution([ + FindPackageShare("ff_viz"), + "launch", + "ff_viz.launch.py", + ]), + launch_arguments={"robot_name": robot_name}.items(), + condition=IfCondition(rviz), + ), Node( package="ff_control", executable="pd_ctrl_node", From 46bb7f187ed4a310ea092e2eba8264ac3a3d5245 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Mon, 22 May 2023 17:10:14 -0700 Subject: [PATCH 53/56] fix typo --- freeflyer/launch/hardware_pd.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index 00996c5..3edf9f0 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): DeclareLaunchArgument("robot_name", default_value="robot"), DeclareLaunchArgument("rviz", default_value="false", description="set to true to launch rviz", - choices=["true", "false"]) + choices=["true", "false"]), IncludeLaunchDescription( PathJoinSubstitution([ FindPackageShare("ff_viz"), From d54214c6f407e163406a318291d77c8c8fafeb4e Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Mon, 22 May 2023 17:13:15 -0700 Subject: [PATCH 54/56] format code --- freeflyer/launch/hardware_pd.launch.py | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index 3edf9f0..3f74177 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -36,17 +36,22 @@ def generate_launch_description(): return LaunchDescription( [ DeclareLaunchArgument("robot_name", default_value="robot"), - DeclareLaunchArgument("rviz", default_value="false", - description="set to true to launch rviz", - choices=["true", "false"]), + DeclareLaunchArgument( + "rviz", + default_value="false", + description="set to true to launch rviz", + choices=["true", "false"], + ), IncludeLaunchDescription( - PathJoinSubstitution([ - FindPackageShare("ff_viz"), - "launch", - "ff_viz.launch.py", - ]), - launch_arguments={"robot_name": robot_name}.items(), - condition=IfCondition(rviz), + PathJoinSubstitution( + [ + FindPackageShare("ff_viz"), + "launch", + "ff_viz.launch.py", + ] + ), + launch_arguments={"robot_name": robot_name}.items(), + condition=IfCondition(rviz), ), Node( package="ff_control", From d51fd882103ef66681c37c2d5c126dcd207cd601 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Mon, 22 May 2023 20:30:05 -0700 Subject: [PATCH 55/56] fix py test --- ff_control/ff_control/linear_ctrl.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ff_control/ff_control/linear_ctrl.py b/ff_control/ff_control/linear_ctrl.py index 598e9eb..6e9d2d4 100644 --- a/ff_control/ff_control/linear_ctrl.py +++ b/ff_control/ff_control/linear_ctrl.py @@ -49,7 +49,7 @@ class LinearController(WrenchController): def __init__(self, node_name="linear_ctrl_node"): super().__init__(node_name) self._state_sub = self.create_subscription( - FreeFlyerStateStamped, "gt/state", self._state_callback, 10 + FreeFlyerStateStamped, "est/state", self._state_callback, 10 ) self._state_ready = False self._state_stamped = FreeFlyerStateStamped() From bf86cd751a3d7d948a7109e0147173129f44426a Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 2 Jun 2023 16:01:50 -0700 Subject: [PATCH 56/56] fix pd ctrl node name for hardware launch --- freeflyer/launch/hardware_pd.launch.py | 9 ++++++++- freeflyer/launch/sim_pd.launch.py | 7 ++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index 3f74177..dcceee7 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -32,6 +32,7 @@ def generate_launch_description(): robot_name = LaunchConfiguration("robot_name") rviz = LaunchConfiguration("rviz") + impl = LaunchConfiguration("impl") return LaunchDescription( [ @@ -42,6 +43,12 @@ def generate_launch_description(): description="set to true to launch rviz", choices=["true", "false"], ), + DeclareLaunchArgument( + "impl", + default_value="cpp", + description="PD controller implementation", + choices=["cpp", "py"], + ), IncludeLaunchDescription( PathJoinSubstitution( [ @@ -55,7 +62,7 @@ def generate_launch_description(): ), Node( package="ff_control", - executable="pd_ctrl_node", + executable=["pd_ctrl_", impl, "_node"], name="pd_ctrl_node", namespace=robot_name, ), diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 24f3985..8f4c333 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -35,7 +35,12 @@ def generate_launch_description(): return LaunchDescription( [ DeclareLaunchArgument("robot_name", default_value="robot"), - DeclareLaunchArgument("impl", default_value="cpp", choices=["cpp", "py"]), + DeclareLaunchArgument( + "impl", + default_value="cpp", + description="PD controller implementation", + choices=["cpp", "py"], + ), IncludeLaunchDescription( PathJoinSubstitution( [