diff --git a/.gitignore b/.gitignore index e8c7c840..3a2330e4 100644 --- a/.gitignore +++ b/.gitignore @@ -246,3 +246,4 @@ MUJOCO_LOG.TXT obk_logs/ docker/user_setup.sh docker/install_sys_deps.sh +docker/obelisk \ No newline at end of file diff --git a/README.md b/README.md index e1416292..3272a796 100644 --- a/README.md +++ b/README.md @@ -75,6 +75,7 @@ Some guidance/recommendations on choosing flags: If you're installing `docker` for the first time using this script, you also need to run afterwards ``` +sudo usermod -aG docker $USER newgrp docker ``` @@ -115,5 +116,15 @@ obk-clean ``` This will delete cached build files associated with Obelisk. If you have tried building the Obelisk source code multiple times or from different environments/local filesystems, it may be corrupted, and cleaning the installation can help fix issues. +To run a ROS stack, run +``` +obk-launch config= device= auto_start= +``` + +For the dummy examples this looks like: +``` +obk-launch config=dummy_cpp.yaml device=onboard +``` + ## Building Docs In the repository root, to build the docs locally, run `sphinx-build -M html docs/source/ docs/build/`. diff --git a/docker/Dockerfile b/docker/Dockerfile index e63b00c3..8d5548bd 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -61,6 +61,7 @@ RUN groupadd --gid $GID $USER && \ # conditionally install dependencies based on build args # source base ROS if basic deps are installed COPY install_sys_deps.sh /tmp/install_sys_deps.sh + RUN FLAGS=""; \ [ "$OBELISK_DOCKER_BASIC" = "true" ] && FLAGS="$FLAGS --basic"; \ [ "$OBELISK_DOCKER_CYCLONE_PERF" = "true" ] && FLAGS="$FLAGS --cyclone-perf"; \ @@ -70,6 +71,11 @@ RUN FLAGS=""; \ bash /tmp/install_sys_deps.sh $FLAGS && \ sudo rm /tmp/install_sys_deps.sh +# Install obelisk_py as editable. +# Copy obelisk code from docker/obelisk/python into the container at . +COPY obelisk/python ./obelisk/python +RUN pip install -e ./obelisk/python + # conditional configure groups based on build args COPY config_groups.sh /tmp/config_groups.sh RUN FLAGS=""; \ diff --git a/docs/source/development.md b/docs/source/development.md index 9d88ecc7..09b1e0c9 100644 --- a/docs/source/development.md +++ b/docs/source/development.md @@ -30,6 +30,7 @@ Sometimes we have found that .bash_aliases is a folder. For this to work, you wi If you have just installed docker for the first time, you may need to run ``` +sudo usermod -aG docker $USER newgrp docker ``` diff --git a/docs/source/obelisk_terminal_aliases.md b/docs/source/obelisk_terminal_aliases.md index 6d5b8eff..be4192ec 100644 --- a/docs/source/obelisk_terminal_aliases.md +++ b/docs/source/obelisk_terminal_aliases.md @@ -31,6 +31,7 @@ Configure all Obelisk nodes. ``` obk-configure ``` +For example, the config_name is `dummy` for dummy_cpp.yaml. ### `obk-activate` Activate all Obelisk nodes. diff --git a/obelisk/cpp/hardware/include/unitree_example_controller.h b/obelisk/cpp/hardware/include/unitree_example_controller.h index 99cfa176..a40906c3 100644 --- a/obelisk/cpp/hardware/include/unitree_example_controller.h +++ b/obelisk/cpp/hardware/include/unitree_example_controller.h @@ -80,7 +80,8 @@ namespace obelisk { } this->GetPublisher(this->ctrl_key_)->publish(msg); - + RCLCPP_INFO_STREAM(this->get_logger(), "msg.u_mujoco.size*(): " << msg.u_mujoco.size()); // FIXME: This is 81 but should be 87 + // because model->nu = 87. return msg; }; diff --git a/obelisk/cpp/hardware/include/unitree_example_estimator.h b/obelisk/cpp/hardware/include/unitree_example_estimator.h index 195289b0..92d1944c 100644 --- a/obelisk/cpp/hardware/include/unitree_example_estimator.h +++ b/obelisk/cpp/hardware/include/unitree_example_estimator.h @@ -9,7 +9,6 @@ namespace obelisk { public: UnitreeExampleEstimator(const std::string& name) : obelisk::ObeliskEstimator(name) { - this->RegisterObkSubscription( "sub_sensor_setting", "sub_sensor", std::bind(&UnitreeExampleEstimator::JointEncoderCallback, this, std::placeholders::_1)); @@ -17,26 +16,50 @@ namespace obelisk { protected: void JointEncoderCallback(const obelisk_sensor_msgs::msg::ObkJointEncoders& msg) { + t_last_update_ = this->now().nanoseconds() / std::pow(10, 9); // time since 1970 joint_encoders_ = msg.joint_pos; joint_vels_ = msg.joint_vel; joint_names_ = msg.joint_names; } obelisk_estimator_msgs::msg::EstimatedState ComputeStateEstimate() override { + // This is called by a timer specified in the .yaml file. + + // If too many seconds has passed without an update, throw an error. + double current_time = this->now().nanoseconds() / std::pow(10, 9); // time since 1970 + double t_without_update = current_time - t_last_update_; + + if (t_last_update_ != 0 + && t_without_update > MAX_TIME_WITHOUT_UPDATE) { + throw std::runtime_error( + "No state estimate received within the last " + + std::to_string(MAX_TIME_WITHOUT_UPDATE) + + " seconds. Robot may have crashed." + ); + } + + // Create the estimated msg obelisk_estimator_msgs::msg::EstimatedState msg; msg.header.stamp = this->now(); - msg.q_joints = joint_encoders_; // Joint Positions - msg.v_joints = joint_vels_; // Joint Velocities - msg.joint_names = joint_names_; // Joint Names - // msg.base_link_name = "link0"; - - this->GetPublisher(this->est_pub_key_)->publish(msg); + msg.q_joints = joint_encoders_; // Joint Positions + msg.v_joints = joint_vels_; // Joint Velocities + msg.joint_names = joint_names_; // Joint Names + msg.base_link_name = BASE_LINK_NAME; + // If there are values stored in the joint encoders, publish + // the estimated message + if (joint_encoders_.size() != 0) { + this->GetPublisher(this->est_pub_key_)->publish(msg); + } return msg; }; private: + const double MAX_TIME_WITHOUT_UPDATE = 1; // seconds + const std::string BASE_LINK_NAME = "base_link"; + + double t_last_update_; // defaults to 0 std::vector joint_encoders_; std::vector joint_vels_; std::vector joint_names_; diff --git a/obelisk/cpp/hardware/include/unitree_go2_estimator.h b/obelisk/cpp/hardware/include/unitree_go2_estimator.h index 47c49d7d..3e4f8919 100644 --- a/obelisk/cpp/hardware/include/unitree_go2_estimator.h +++ b/obelisk/cpp/hardware/include/unitree_go2_estimator.h @@ -41,7 +41,7 @@ namespace obelisk { msg.joint_names = joint_names_; // Joint Names msg.q_base = pose_; // Quaternion msg.v_base = omega_; // Angular Velocity - // msg.base_link_name = "link0"; + // msg.base_link_name = "base_link"; this->GetPublisher(this->est_pub_key_)->publish(msg); diff --git a/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt b/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt index 204aed9d..b85c29c6 100644 --- a/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt +++ b/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt @@ -6,27 +6,40 @@ message(STATUS "Configuring Unitree Interface") find_package(ament_cmake REQUIRED) include(FetchContent) + +# Let `unitree_sdk` refer to the content fetched from the git repo. FetchContent_Declare( unitree_sdk GIT_REPOSITORY https://github.com/unitreerobotics/unitree_sdk2.git - GIT_TAG 3a4680ae9b00df59e60f7e63cfb0fcc432a9d08d) # main # TODO: Fix the issue here - -set(UNITREE_SDK_BUILD_EXAMPLES OFF CACHE BOOL "Disable building examples") + GIT_TAG 3a4680ae9b00df59e60f7e63cfb0fcc432a9d08d) # NOTE: this isn't the most recent commit for the unitree_sdk2 + +# Couldn't find `UNITREE_SDK_BUILD_EXAMPLES` being defined in unitree_sdk2. +# set(UNITREE_SDK_BUILD_EXAMPLES OFF CACHE BOOL "Disable building examples") +# `BUILD_EXAMPLES` is declared in unitree_sdk2 repo's CMakeLists.txt set(BUILD_EXAMPLES OFF CACHE BOOL "Disable building examples") FetchContent_MakeAvailable(unitree_sdk) +# Adds an Object Library to compile source files without linking their +# object files into a library add_library(UnitreeInterface INTERFACE) + +# Adds the current directory (.) to the include path for the UnitreeInterface library target_include_directories(UnitreeInterface INTERFACE .) -target_link_libraries(UnitreeInterface INTERFACE Obelisk::Core unitree_sdk2) +# Specifies that UnitreeInterface depends on two libraries: Obelisk::Core and unitree_sdk2. +# unitree_sdk2 is the CMake target name defined by unitree_sdk2 repo's CMakeLists.txt. +target_link_libraries(UnitreeInterface INTERFACE + Obelisk::Core + unitree_sdk2 +) +# Adds ROS 2 dependencies to UnitreeInterface ament_target_dependencies(UnitreeInterface INTERFACE rclcpp rclcpp_lifecycle sensor_msgs) - # add_executable(unitree_test unitree_test.cpp) # target_link_libraries(unitree_test unitree_sdk2) diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index 3a107ded..f935f3e6 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -55,7 +55,7 @@ namespace obelisk { } protected: void CreateUnitreePublishers() override { - // create low level command publisher + // Create low level command publisher lowcmd_publisher_.reset(new ChannelPublisher(CMD_TOPIC_)); lowcmd_publisher_->InitChannel(); @@ -63,7 +63,7 @@ namespace obelisk { } void CreateUnitreeSubscribers() override { - // Dreate unitree subscriber + // Create unitree subscriber // Need to create after the publishers have been activated lowstate_subscriber_.reset(new ChannelSubscriber(STATE_TOPIC_)); lowstate_subscriber_->InitChannel(std::bind(&Go2Interface::LowStateHandler, this, std::placeholders::_1), 1); @@ -72,7 +72,7 @@ namespace obelisk { } void ApplyControl(const unitree_control_msg& msg) override { - // Only execute of in Low Level Control or Home modes + // Only execute if in Low Level Control or Home modes if (exec_fsm_state_ != ExecFSMState::LOW_LEVEL_CTRL && exec_fsm_state_ != ExecFSMState::USER_POSE) { return; } @@ -306,7 +306,7 @@ namespace obelisk { std::string ODOM_TOPIC_; - unitree::robot::ChannelPublisherPtr lowcmd_publisher_; + ChannelPublisherPtr lowcmd_publisher_; ChannelSubscriberPtr lowstate_subscriber_; // ChannelSubscriberPtr odom_subscriber_; diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index f526c38b..80527b21 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -47,15 +47,19 @@ namespace obelisk { */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& prev_state) { + RCLCPP_INFO_STREAM(this->get_logger(), "Configuring the ObeliskSimRobot robot"); this->ObeliskSimRobot::on_configure(prev_state); // Read in the config string + RCLCPP_INFO_STREAM(this->get_logger(), "Reading in config stream"); std::string mujoco_setting = this->get_parameter("mujoco_setting").as_string(); auto mujoco_config_map = this->ParseConfigStr(mujoco_setting); // Get config params + RCLCPP_INFO_STREAM(this->get_logger(), "Getting config params"); xml_path_ = GetXMLPath(mujoco_config_map); // Required std::string robot_pkg = GetRobotPackage(mujoco_config_map); // Optional + // Search for the model if (!std::filesystem::exists(xml_path_)) { if (robot_pkg != "None" && robot_pkg != "none") { @@ -89,7 +93,7 @@ namespace obelisk { } catch (const std::exception& e) { RCLCPP_INFO_STREAM(this->get_logger(), "No geoms to visualize in the simulator."); } - + RCLCPP_INFO_STREAM(this->get_logger(), "Configured Obelisk mujoco sim robot"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -103,7 +107,7 @@ namespace obelisk { this->ObeliskSimRobot::on_activate(prev_state); activation_complete_ = true; - + RCLCPP_INFO_STREAM(this->get_logger(), "Activated the mujoco robot"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -185,7 +189,6 @@ namespace obelisk { nu_ = model_->nu; RCLCPP_INFO_STREAM(this->get_logger(), "Mujoco model loaded with " << nu_ << " inputs."); shared_data_.resize(nu_); - if (!model_) { throw std::runtime_error("Could not load Mujoco model from the XML!"); } @@ -215,9 +218,10 @@ namespace obelisk { shared_data_tmp.push_back(data_->ctrl[i]); } SetSharedData(shared_data_tmp); + break; } } - + while (!this->stop_thread_) { auto start_time = this->now(); { @@ -227,7 +231,7 @@ namespace obelisk { data_->ctrl[i] = shared_data_.at(i); } } - + { std::lock_guard lock(sensor_data_mut_); mj_step(model_, data_); @@ -238,12 +242,10 @@ namespace obelisk { while ((this->now() - start_time).nanoseconds() < time_step_ * 1e9) { } } - RCLCPP_WARN_STREAM(this->get_logger(), "Cleaning up simulation data and model..."); // free MuJoCo model and data mj_deleteData(data_); mj_deleteModel(model_); - rendering_thread_.join(); } diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h index 9bac807f..c7dd19da 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h @@ -23,8 +23,8 @@ namespace obelisk { } /** - * @brief Configures all the required ROS components. Specifcially this - * registers the control_subscriver_. Also makes a call to ObeliskNode on configure + * @brief Configures all the required ROS components. Specifically this + * registers the control_subscriber_. Also makes a call to ObeliskNode on configure * to parse and create the callback group map. */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_configure( diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_ros_utils.h b/obelisk/cpp/obelisk_cpp/include/obelisk_ros_utils.h index bfefd9cb..5c2f6515 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_ros_utils.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_ros_utils.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "rclcpp/rclcpp.hpp" @@ -15,14 +16,32 @@ namespace obelisk::utils { * @param name the name to give to the node */ template void SpinObelisk(int argc, char* argv[], const std::string& name) { - rclcpp::init(argc, argv); - + if (!rclcpp::ok()) { // check if rclpy is not initialized + rclcpp::init(argc, argv); + } auto node = std::make_shared(name); - - ExecutorT executor; + RCLCPP_INFO(node->get_logger(), "Initializing node: %s", name.c_str()); + ExecutorT executor; executor.add_node(node->get_node_base_interface()); - executor.spin(); + try { + executor.spin(); + } + catch (const std::exception& e) { + // Catch standard exceptions + RCLCPP_ERROR(node->get_logger(), "Exception during spin: %s", e.what()); + } + catch (...) { + // Catch any other unexpected exceptions + RCLCPP_ERROR(node->get_logger(), "Unknown exception during spin"); + } + + // Cleanup executor.remove_node(node->get_node_base_interface()); - rclcpp::shutdown(); + node.reset(); // destroy the node + + // Shutdown rclcpp if context is still valid + if (rclcpp::ok()) { + rclcpp::shutdown(); + } } } // namespace obelisk::utils diff --git a/obelisk/python/obelisk_py/core/control.py b/obelisk/python/obelisk_py/core/control.py index 7d04db5a..516298a7 100644 --- a/obelisk/python/obelisk_py/core/control.py +++ b/obelisk/python/obelisk_py/core/control.py @@ -17,7 +17,9 @@ class ObeliskController(ABC, ObeliskNode): the control message should be of type ObeliskControlMsg to be compatible with the Obelisk ecosystem. """ - def __init__(self, node_name: str, ctrl_msg_type: Type, est_msg_type: Type) -> None: + def __init__( + self, node_name: str, ctrl_msg_type: Type, est_msg_type: Type + ) -> None: """Initialize the Obelisk controller.""" super().__init__(node_name) self.register_obk_timer( diff --git a/obelisk/python/obelisk_py/core/node.py b/obelisk/python/obelisk_py/core/node.py index dfa76f33..fc4db933 100644 --- a/obelisk/python/obelisk_py/core/node.py +++ b/obelisk/python/obelisk_py/core/node.py @@ -1,7 +1,21 @@ -from typing import Any, Callable, Dict, List, Optional, Tuple, Type, TypeVar, Union +from typing import ( + Any, + Callable, + Dict, + List, + Optional, + Tuple, + Type, + TypeVar, + Union, +) # noqa: I001 import rclpy -from rclpy.callback_groups import CallbackGroup, MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.callback_groups import ( + CallbackGroup, + MutuallyExclusiveCallbackGroup, + ReentrantCallbackGroup, +) from rclpy.lifecycle import LifecycleNode from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn @@ -49,6 +63,10 @@ class ObeliskNode(LifecycleNode): def __init__(self, node_name: str) -> None: """Initialize the Obelisk node.""" super().__init__(node_name) + self.info = self.get_logger().info + self.warn = self.get_logger().warn + self.error = self.get_logger().error + self.declare_parameter("callback_group_settings", "") # ROS parameter designed to let the user feed a file path for their own code self.declare_parameter("params_path", "") @@ -100,7 +118,9 @@ def register_obk_publisher( self.declare_parameter(ros_parameter, rclpy.Parameter.Type.STRING) else: self.declare_parameter(ros_parameter, value=default_config_str) - self._obk_pub_settings.append({"key": key, "ros_parameter": ros_parameter, "msg_type": msg_type}) + self._obk_pub_settings.append( + {"key": key, "ros_parameter": ros_parameter, "msg_type": msg_type} + ) def register_obk_subscription( self, @@ -128,7 +148,12 @@ def register_obk_subscription( else: self.declare_parameter(ros_parameter, value=default_config_str) self._obk_sub_settings.append( - {"key": key, "ros_parameter": ros_parameter, "callback": callback, "msg_type": msg_type} + { + "key": key, + "ros_parameter": ros_parameter, + "callback": callback, + "msg_type": msg_type, + } ) def register_obk_timer( @@ -153,14 +178,18 @@ def register_obk_timer( self.declare_parameter(ros_parameter, rclpy.Parameter.Type.STRING) else: self.declare_parameter(ros_parameter, value=default_config_str) - self._obk_timer_settings.append({"key": key, "ros_parameter": ros_parameter, "callback": callback}) + self._obk_timer_settings.append( + {"key": key, "ros_parameter": ros_parameter, "callback": callback} + ) # ############## # # STATIC METHODS # # ############## # @staticmethod - def _parse_config_str(config_str: str) -> Tuple[List[str], List[Union[str, float, int]]]: + def _parse_config_str( + config_str: str, + ) -> Tuple[List[str], List[Union[str, float, int]]]: """Parse a configuration string into a list of field names and a list of values. Parameters: @@ -190,9 +219,13 @@ def _convert_values(value: str) -> Union[str, float, int]: return value try: - field_names, value_names = list(zip(*[pair.split(":") for pair in field_value_pairs])) # split by colon + field_names, value_names = list( + zip(*[pair.split(":") for pair in field_value_pairs]) + ) # split by colon field_names = list(field_names) # Convert to list - value_names = [_convert_values(value) for value in value_names] # convert ints/floats + value_names = [ + _convert_values(value) for value in value_names + ] # convert ints/floats except ValueError as e: raise ValueError( "Each field-value pair in the configuration string must be separated by a colon!\n" @@ -202,7 +235,11 @@ def _convert_values(value: str) -> Union[str, float, int]: return field_names, value_names @staticmethod - def _check_fields(field_names: List[str], required_field_names: List[str], optional_field_names: List[str]) -> None: + def _check_fields( + field_names: List[str], + required_field_names: List[str], + optional_field_names: List[str], + ) -> None: """Check if a configuration string is valid. Parameters: @@ -213,18 +250,26 @@ def _check_fields(field_names: List[str], required_field_names: List[str], optio Raises: AssertionError: If the configuration string is invalid. """ + assert all([field in field_names for field in required_field_names]), ( + f"config_str must contain the following fields: {required_field_names}" + ) assert all( - [field in field_names for field in required_field_names] - ), f"config_str must contain the following fields: {required_field_names}" - assert all([field in required_field_names + optional_field_names for field in field_names]), ( + [ + field in required_field_names + optional_field_names + for field in field_names + ] + ), ( f"""The following fields in the config_str are invalid: { - set(field_names) - set(required_field_names + optional_field_names) + set(field_names) + - set(required_field_names + optional_field_names) }""", f"Currently-supported fields in Obelisk are: {required_field_names + optional_field_names}", ) @staticmethod - def _check_values(value_names: List[str], allowable_value_names: List[str]) -> None: + def _check_values( + value_names: List[str], allowable_value_names: List[str] + ) -> None: """Check if the values in a configuration string are valid. Parameters: @@ -245,11 +290,15 @@ def _check_values(value_names: List[str], allowable_value_names: List[str]) -> N def _get_key_from_config_dict(config_dict: Dict) -> str: """Get the key from a configuration dictionary.""" assert "key" in config_dict, "No key supplied!" - assert isinstance(config_dict["key"], str), "The 'key' field must be a string!" + assert isinstance(config_dict["key"], str), ( + "The 'key' field must be a string!" + ) return config_dict["key"] @staticmethod - def _create_callback_groups_from_config_str(config_str: str) -> Dict[str, CallbackGroup]: + def _create_callback_groups_from_config_str( + config_str: str, + ) -> Dict[str, CallbackGroup]: """Create callback groups from a configuration string. Parameters: @@ -271,7 +320,10 @@ def _create_callback_groups_from_config_str(config_str: str) -> Dict[str, Callba if not field_names and not value_names: return {} # case: no callback groups to create - allowable_value_names = ["MutuallyExclusiveCallbackGroup", "ReentrantCallbackGroup"] + allowable_value_names = [ + "MutuallyExclusiveCallbackGroup", + "ReentrantCallbackGroup", + ] ObeliskNode._check_values(value_names, allowable_value_names) config_dict = dict(zip(field_names, value_names)) @@ -283,7 +335,9 @@ def _create_callback_groups_from_config_str(config_str: str) -> Dict[str, Callba elif callback_group_type == "ReentrantCallbackGroup": callback_group = ReentrantCallbackGroup() else: - raise ValueError(f"Invalid callback group type: {callback_group_type}") + raise ValueError( + f"Invalid callback group type: {callback_group_type}" + ) callback_group_dict[callback_group_name] = callback_group return callback_group_dict @@ -292,16 +346,22 @@ def _create_callback_groups_from_config_str(config_str: str) -> Dict[str, Callba # INSTANCE-SPECIFIC UTILS # # ####################### # - def _get_callback_group_from_config_dict(self, config_dict: Dict) -> Optional[CallbackGroup]: + def _get_callback_group_from_config_dict( + self, config_dict: Dict + ) -> Optional[CallbackGroup]: """Get the callback group from a configuration dictionary.""" if "callback_group" in config_dict: - assert isinstance(config_dict["callback_group"], str), "The 'callback_group' field must be a string!" + assert isinstance(config_dict["callback_group"], str), ( + "The 'callback_group' field must be a string!" + ) if config_dict["callback_group"].lower() == "none": return None else: - cbg = self.obk_callback_groups.get(config_dict["callback_group"], None) + cbg = self.obk_callback_groups.get( + config_dict["callback_group"], None + ) if cbg is None: - self.get_logger().warn( + self.warn( f"Callback group {config_dict['callback_group']} not found in node. Using None instead." ) return cbg @@ -338,15 +398,27 @@ def _create_publisher_from_config_str( # parse and check the configuration string field_names, value_names = ObeliskNode._parse_config_str(config_str) required_field_names = ["topic"] - optional_field_names = ["key", "msg_type", "history_depth", "callback_group"] - ObeliskNode._check_fields(field_names, required_field_names, optional_field_names) + optional_field_names = [ + "key", + "msg_type", + "history_depth", + "callback_group", + ] + ObeliskNode._check_fields( + field_names, required_field_names, optional_field_names + ) config_dict = dict(zip(field_names, value_names)) # parse the key if key is None: - key = ObeliskNode._get_key_from_config_dict(config_dict) + try: + key = ObeliskNode._get_key_from_config_dict(config_dict) + except AssertionError as e: + self.error( + "Failed to extract key from publisher config dict: %s" % e + ) elif "key" in field_names: - self.get_logger().warn( + self.warn( f"'key'={key} registered for this publisher, and 'key'={config_dict['key']} specified in the config " f"string. Using the value 'key'={key}, as hardcoded specifications take precedence!" ) @@ -356,16 +428,24 @@ def _create_publisher_from_config_str( # run type assertions and create the publisher history_depth = config_dict.get("history_depth", 10) - assert isinstance(config_dict["topic"], str), "The 'topic' field must be a string!" - assert isinstance(history_depth, int), "The 'history_depth' field must be an int!" + assert isinstance(config_dict["topic"], str), ( + "The 'topic' field must be a string!" + ) + assert isinstance(history_depth, int), ( + "The 'history_depth' field must be an int!" + ) self.obk_publishers[key] = self.create_publisher( msg_type=msg_type, topic=config_dict["topic"], qos_profile=history_depth, callback_group=callback_group, ) - assert not hasattr(self, key), f"Attribute {key} already exists in the node!" - setattr(self, key + "_key", self.obk_publishers[key]) # create key attribute for publisher + assert not hasattr(self, key), ( + f"Attribute {key} already exists in the node!" + ) + setattr( + self, key + "_key", self.obk_publishers[key] + ) # create key attribute for publisher return key def _create_subscription_from_config_str( @@ -401,15 +481,28 @@ def _create_subscription_from_config_str( # parse and check the configuration string field_names, value_names = ObeliskNode._parse_config_str(config_str) required_field_names = ["topic"] - optional_field_names = ["key", "msg_type", "history_depth", "callback_group"] - ObeliskNode._check_fields(field_names, required_field_names, optional_field_names) + optional_field_names = [ + "key", + "msg_type", + "history_depth", + "callback_group", + ] + ObeliskNode._check_fields( + field_names, required_field_names, optional_field_names + ) config_dict = dict(zip(field_names, value_names)) # parse the key if key is None: - key = ObeliskNode._get_key_from_config_dict(config_dict) + try: + key = ObeliskNode._get_key_from_config_dict(config_dict) + except AssertionError as e: + self.error( + "Failed to extract key from subscription config dict: %s" + % e + ) elif "key" in field_names: - self.get_logger().warn( + self.warn( f"'key'={key} registered for this subscription, and 'key'={config_dict['key']} specified in the config " f"string. Using the value 'key'={key}, as hardcoded specifications take precedence!" ) @@ -419,8 +512,12 @@ def _create_subscription_from_config_str( # run type assertions and return the subscription history_depth = config_dict.get("history_depth", 10) - assert isinstance(config_dict["topic"], str), "The 'topic' field must be a string!" - assert isinstance(history_depth, int), "The 'history_depth' field must be an int!" + assert isinstance(config_dict["topic"], str), ( + "The 'topic' field must be a string!" + ) + assert isinstance(history_depth, int), ( + "The 'history_depth' field must be an int!" + ) self.obk_subscriptions[key] = self.create_subscription( msg_type=msg_type, @@ -429,8 +526,12 @@ def _create_subscription_from_config_str( qos_profile=history_depth, callback_group=callback_group, ) - assert not hasattr(self, key), f"Attribute {key} already exists in the node!" - setattr(self, key + "_key", self.obk_subscriptions[key]) # create key attribute for subscription + assert not hasattr(self, key), ( + f"Attribute {key} already exists in the node!" + ) + setattr( + self, key + "_key", self.obk_subscriptions[key] + ) # create key attribute for subscription return key def _create_timer_from_config_str( @@ -459,14 +560,19 @@ def _create_timer_from_config_str( field_names, value_names = ObeliskNode._parse_config_str(config_str) required_field_names = ["timer_period_sec"] optional_field_names = ["key", "callback_group"] - ObeliskNode._check_fields(field_names, required_field_names, optional_field_names) + ObeliskNode._check_fields( + field_names, required_field_names, optional_field_names + ) config_dict = dict(zip(field_names, value_names)) # parse the key if key is None: - key = ObeliskNode._get_key_from_config_dict(config_dict) + try: + key = ObeliskNode._get_key_from_config_dict(config_dict) + except AssertionError as e: + self.error("Failed to extract key from timer config: %s" % e) elif "key" in field_names: - self.get_logger().warn( + self.warn( f"'key'={key} registered for this timer, and 'key'={config_dict['key']} specified in the config " f"string. Using the value 'key'={key}, as hardcoded specifications take precedence!" ) @@ -475,9 +581,9 @@ def _create_timer_from_config_str( callback_group = self._get_callback_group_from_config_dict(config_dict) # run type assertions and return the timer - assert isinstance( - config_dict["timer_period_sec"], (int, float) - ), "The 'timer_period_sec' field must be a number!" + assert isinstance(config_dict["timer_period_sec"], (int, float)), ( + "The 'timer_period_sec' field must be a number!" + ) timer = self.create_timer( config_dict["timer_period_sec"], @@ -487,8 +593,12 @@ def _create_timer_from_config_str( ) timer.cancel() # initially, the timer should be deactivated, TODO(ahl): remove if distro upgraded self.obk_timers[key] = timer - assert not hasattr(self, key), f"Attribute {key} already exists in the node!" - setattr(self, key + "_key", self.obk_timers[key]) # create key attribute for timer + assert not hasattr(self, key), ( + f"Attribute {key} already exists in the node!" + ) + setattr( + self, key + "_key", self.obk_timers[key] + ) # create key attribute for timer return key # ################### # @@ -504,11 +614,22 @@ def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: super().on_configure(state) # parsing config strings - callback_group_settings = self.get_parameter("callback_group_settings").get_parameter_value().string_value + callback_group_settings = ( + self.get_parameter("callback_group_settings") + .get_parameter_value() + .string_value + ) # create callback groups - self.obk_callback_groups = ObeliskNode._create_callback_groups_from_config_str(callback_group_settings) - for callback_group_name, callback_group in self.obk_callback_groups.items(): + self.obk_callback_groups = ( + ObeliskNode._create_callback_groups_from_config_str( + callback_group_settings + ) + ) + for ( + callback_group_name, + callback_group, + ) in self.obk_callback_groups.items(): setattr(self, callback_group_name, callback_group) # create components @@ -517,12 +638,20 @@ def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: ros_parameter = pub_dict["ros_parameter"] msg_type = pub_dict["msg_type"] - pub_config_str = self.get_parameter(ros_parameter).get_parameter_value().string_value + pub_config_str = ( + self.get_parameter(ros_parameter) + .get_parameter_value() + .string_value + ) if pub_config_str == "": - self.get_logger().warn(f"Publisher {key} has no configuration string!") + self.warn(f"Publisher {key} has no configuration string!") continue - final_key = self._create_publisher_from_config_str(pub_config_str, key=key, msg_type=msg_type) - pub_dict["key"] = final_key # if no key passed, use value from config file + final_key = self._create_publisher_from_config_str( + pub_config_str, key=key, msg_type=msg_type + ) + pub_dict["key"] = ( + final_key # if no key passed, use value from config file + ) for sub_dict in self._obk_sub_settings: key = sub_dict["key"] @@ -530,28 +659,42 @@ def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: msg_type = sub_dict["msg_type"] callback = sub_dict["callback"] - sub_config_str = self.get_parameter(ros_parameter).get_parameter_value().string_value + sub_config_str = ( + self.get_parameter(ros_parameter) + .get_parameter_value() + .string_value + ) if sub_config_str == "": - self.get_logger().warn(f"Subscription {key} has no configuration string!") + self.warn(f"Subscription {key} has no configuration string!") continue final_key = self._create_subscription_from_config_str( sub_config_str, callback=callback, key=key, msg_type=msg_type ) - sub_dict["key"] = final_key # if no key passed, use value from config file + sub_dict["key"] = ( + final_key # if no key passed, use value from config file + ) for timer_dict in self._obk_timer_settings: key = timer_dict["key"] ros_parameter = timer_dict["ros_parameter"] callback = timer_dict["callback"] - timer_config_str = self.get_parameter(ros_parameter).get_parameter_value().string_value + timer_config_str = ( + self.get_parameter(ros_parameter) + .get_parameter_value() + .string_value + ) if timer_config_str == "": - self.get_logger().warn(f"Timer {key} has no configuration string!") + self.warn(f"Timer {key} has no configuration string!") continue - final_key = self._create_timer_from_config_str(timer_config_str, callback=callback, key=key) - timer_dict["key"] = final_key # if no key passed, use value from config file + final_key = self._create_timer_from_config_str( + timer_config_str, callback=callback, key=key + ) + timer_dict["key"] = ( + final_key # if no key passed, use value from config file + ) - self.get_logger().info(f"{self.get_name()} configured.") + self.info(f"{self.get_name()} configured.") return TransitionCallbackReturn.SUCCESS def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: @@ -560,7 +703,7 @@ def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: for timer in self.obk_timers.values(): timer.reset() # activate timers - self.get_logger().info(f"{self.get_name()} activated.") + self.info(f"{self.get_name()} activated.") return TransitionCallbackReturn.SUCCESS def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: @@ -569,7 +712,7 @@ def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: for timer in self.obk_timers.values(): timer.cancel() # deactivate timers - self.get_logger().info(f"{self.get_name()} deactivated.") + self.info(f"{self.get_name()} deactivated.") return TransitionCallbackReturn.SUCCESS def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn: @@ -595,12 +738,12 @@ def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn: for subscription in self.subscriptions: self.destroy_subscription(subscription) - self.get_logger().info(f"{self.get_name()} cleaned up") + self.info(f"{self.get_name()} cleaned up") return TransitionCallbackReturn.SUCCESS def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: """Shut down the controller.""" super().on_shutdown(state) self.on_cleanup(state) - self.get_logger().info(f"{self.get_name()} shut down.") + self.info(f"{self.get_name()} shut down.") return TransitionCallbackReturn.SUCCESS diff --git a/obelisk/python/obelisk_py/core/obelisk_typing.py b/obelisk/python/obelisk_py/core/obelisk_typing.py index 5499e572..2afab851 100644 --- a/obelisk/python/obelisk_py/core/obelisk_typing.py +++ b/obelisk/python/obelisk_py/core/obelisk_typing.py @@ -44,11 +44,22 @@ def is_in_bound(type: Type, typevar: TypeVar) -> bool: oem_classes = get_classes_in_module(oem) osm_classes = get_classes_in_module(osm) -ObeliskControlMsg = TypeVar("ObeliskControlMsg", bound=_create_union_type(ocm_classes)) # type: ignore -ObeliskEstimatorMsg = TypeVar("ObeliskEstimatorMsg", bound=_create_union_type(oem_classes)) # type: ignore -ObeliskSensorMsg = TypeVar("ObeliskSensorMsg", bound=_create_union_type(osm_classes)) # type: ignore -ObeliskMsg = TypeVar("ObeliskMsg", bound=_create_union_type(ocm_classes + oem_classes + osm_classes)) # type: ignore +ObeliskControlMsg = TypeVar( + "ObeliskControlMsg", bound=_create_union_type(ocm_classes) +) # type: ignore +ObeliskEstimatorMsg = TypeVar( + "ObeliskEstimatorMsg", bound=_create_union_type(oem_classes) +) # type: ignore +ObeliskSensorMsg = TypeVar( + "ObeliskSensorMsg", bound=_create_union_type(osm_classes) +) # type: ignore +ObeliskMsg = TypeVar( + "ObeliskMsg", + bound=_create_union_type(ocm_classes + oem_classes + osm_classes), +) # type: ignore ObeliskAllowedMsg = TypeVar( "ObeliskAllowedMsg", - bound=_create_union_type(ocm_classes + oem_classes + osm_classes + [ParameterEvent]), # type: ignore + bound=_create_union_type( + ocm_classes + oem_classes + osm_classes + [ParameterEvent] + ), # type: ignore ) # [NOTE] ParameterEvent is a special case - all nodes have a ParameterEvent publisher in ROS2 diff --git a/obelisk/python/obelisk_py/core/robot.py b/obelisk/python/obelisk_py/core/robot.py index 80b55b42..7620f74e 100644 --- a/obelisk/python/obelisk_py/core/robot.py +++ b/obelisk/python/obelisk_py/core/robot.py @@ -80,7 +80,9 @@ def _set_shared_ctrl(self, ctrl: List[float]) -> None: Parameters: ctrl: The control array of length n_u. """ - assert self.shared_ctrl is not None, "Shared control array must be initialized in derived class!" + assert self.shared_ctrl is not None, ( + "Shared control array must be initialized in derived class!" + ) if hasattr(self, "lock"): with self.lock: self.shared_ctrl[:] = ctrl @@ -90,10 +92,16 @@ def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: super().on_configure(state) # checking the settings of the true sim state pub/timer - if "publisher_true_sim_state" in self.obk_publishers and "timer_true_sim_state" in self.obk_timers: + if ( + "publisher_true_sim_state" in self.obk_publishers + and "timer_true_sim_state" in self.obk_timers + ): assert ( - self.obk_timers["timer_true_sim_state"].callback == self.publish_true_sim_state - ), f"Timer callback must be publish_true_sim_state! Is {self.obk_timers['timer_true_sim_state'].callback}." + self.obk_timers["timer_true_sim_state"].callback + == self.publish_true_sim_state + ), ( + f"Timer callback must be publish_true_sim_state! Is {self.obk_timers['timer_true_sim_state'].callback}." + ) else: self.timer_true_sim_state = None self.publisher_true_sim_state = None diff --git a/obelisk/python/obelisk_py/core/sensing.py b/obelisk/python/obelisk_py/core/sensing.py index 4a3761e4..d41b4dc9 100644 --- a/obelisk/python/obelisk_py/core/sensing.py +++ b/obelisk/python/obelisk_py/core/sensing.py @@ -28,5 +28,7 @@ def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: if msg_type in get_classes_in_module(osm): self._has_sensor_publisher = True break - assert self._has_sensor_publisher, "At least one sensor publisher is required in an ObeliskSensor!" + assert self._has_sensor_publisher, ( + "At least one sensor publisher is required in an ObeliskSensor!" + ) return TransitionCallbackReturn.SUCCESS diff --git a/obelisk/python/obelisk_py/core/utils/internal.py b/obelisk/python/obelisk_py/core/utils/internal.py index de79177d..56909e60 100644 --- a/obelisk/python/obelisk_py/core/utils/internal.py +++ b/obelisk/python/obelisk_py/core/utils/internal.py @@ -12,11 +12,17 @@ def get_classes_in_module(module: ModuleType) -> list[Type]: Returns: A list of classes in the module. """ - classes = [getattr(module, member) for member in dir(module) if inspect.isclass(getattr(module, member))] + classes = [ + getattr(module, member) + for member in dir(module) + if inspect.isclass(getattr(module, member)) + ] return classes -def check_and_get_obelisk_msg_type(msg_type_name: str, msg_module_or_type: Union[ModuleType, TypeVar]) -> Type: +def check_and_get_obelisk_msg_type( + msg_type_name: str, msg_module_or_type: Union[ModuleType, TypeVar] +) -> Type: """Check if a message type is in a module and add it as an attribute to a node in place. Parameters: @@ -34,27 +40,39 @@ def check_and_get_obelisk_msg_type(msg_type_name: str, msg_module_or_type: Union if isinstance(msg_module_or_type, TypeVar): if get_origin(msg_module_or_type.__bound__) is Union: - msg_module_type_names = [a.__name__ for a in get_args(msg_module_or_type.__bound__)] - assert msg_type_name in msg_module_type_names, f"{msg_type_name} must be one of {msg_module_type_names}" + msg_module_type_names = [ + a.__name__ for a in get_args(msg_module_or_type.__bound__) + ] + assert msg_type_name in msg_module_type_names, ( + f"{msg_type_name} must be one of {msg_module_type_names}" + ) for a in get_args(msg_module_or_type.__bound__): if msg_type_name == a.__name__: msg_type = a break else: - assert msg_module_or_type.__bound__ is not None, "The TypeVar does not have a bound." - assert ( - msg_type_name == msg_module_or_type.__bound__.__name__ - ), f"{msg_type_name} must be {msg_module_or_type.__bound__.__name__}" + assert msg_module_or_type.__bound__ is not None, ( + "The TypeVar does not have a bound." + ) + assert msg_type_name == msg_module_or_type.__bound__.__name__, ( + f"{msg_type_name} must be {msg_module_or_type.__bound__.__name__}" + ) msg_type = msg_module_or_type.__bound__ else: - msg_module_type_names = [t.__name__ for t in get_classes_in_module(msg_module_or_type)] - assert msg_type_name in msg_module_type_names, f"{msg_type_name} must be one of {msg_module_type_names}" + msg_module_type_names = [ + t.__name__ for t in get_classes_in_module(msg_module_or_type) + ] + assert msg_type_name in msg_module_type_names, ( + f"{msg_type_name} must be one of {msg_module_type_names}" + ) for msg_module_type_name in msg_module_type_names: if msg_type_name == msg_module_type_name: msg_type = getattr(msg_module_or_type, msg_type_name) break - assert msg_type is not None, f"Could not find {msg_type_name} in {msg_module_or_type}!" + assert msg_type is not None, ( + f"Could not find {msg_type_name} in {msg_module_or_type}!" + ) return msg_type diff --git a/obelisk/python/obelisk_py/core/utils/launch_utils.py b/obelisk/python/obelisk_py/core/utils/launch_utils.py index 2bb8e818..d290b985 100644 --- a/obelisk/python/obelisk_py/core/utils/launch_utils.py +++ b/obelisk/python/obelisk_py/core/utils/launch_utils.py @@ -4,18 +4,25 @@ from pathlib import Path from typing import Dict, List, Optional, Union -import launch -import launch_ros -import lifecycle_msgs.msg from ament_index_python.packages import get_package_share_directory -from launch.actions import EmitEvent, IncludeLaunchDescription, RegisterEventHandler +from launch.actions import ( + EmitEvent, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessStart +from launch.events import matches_action from launch.launch_description_sources import FrontendLaunchDescriptionSource from launch_ros.actions import LifecycleNode, Node +from launch_ros.event_handlers.on_state_transition import OnStateTransition from launch_ros.events.lifecycle import ChangeState +from lifecycle_msgs.msg import Transition from ruamel.yaml import YAML -def load_config_file(file_path: Union[str, Path], package_name: Optional[str] = None) -> Dict: +def load_config_file( + file_path: Union[str, Path], package_name: Optional[str] = None +) -> Dict: """Loads an Obelisk configuration file. Parameters: @@ -30,7 +37,9 @@ def load_config_file(file_path: Union[str, Path], package_name: Optional[str] = file_path = str(file_path) if not file_path.startswith("/"): try: - obk_ros_dir = get_package_share_directory("obelisk_ros" if package_name is None else package_name) + obk_ros_dir = get_package_share_directory( + "obelisk_ros" if package_name is None else package_name + ) abs_file_path = Path(obk_ros_dir) / "config" / file_path except Exception as e: raise FileNotFoundError( @@ -45,10 +54,14 @@ def load_config_file(file_path: Union[str, Path], package_name: Optional[str] = try: return yaml.load(abs_file_path) except Exception as e: - raise FileNotFoundError(f"Could not load a configuration file at {abs_file_path}!") from e + raise FileNotFoundError( + f"Could not load a configuration file at {abs_file_path}!" + ) from e -def get_component_settings_subdict(node_settings: Dict, subdict_name: str) -> Dict: +def get_component_settings_subdict( + node_settings: Dict, subdict_name: str +) -> Dict: """Returns a subdictionary of the component settings associated with an Obelisk node. Parameters: @@ -62,13 +75,30 @@ def get_component_settings_subdict(node_settings: Dict, subdict_name: str) -> Di # iterate over the settings for each component - if doesn't exist, make a new dict entry, else append it for component_settings in node_settings[subdict_name]: - if component_settings["ros_parameter"] not in component_settings_subdict: + if ( + component_settings["ros_parameter"] + not in component_settings_subdict + ): component_settings_subdict[component_settings["ros_parameter"]] = [ - ",".join([f"{k}:{v}" for k, v in component_settings.items() if k != "ros_parameter"]) + ",".join( + [ + f"{k}:{v}" + for k, v in component_settings.items() + if k != "ros_parameter" + ] + ) ] else: - component_settings_subdict[component_settings["ros_parameter"]].append( - ",".join([f"{k}:{v}" for k, v in component_settings.items() if k != "ros_parameter"]) + component_settings_subdict[ + component_settings["ros_parameter"] + ].append( + ",".join( + [ + f"{k}:{v}" + for k, v in component_settings.items() + if k != "ros_parameter" + ] + ) ) # if there is only one setting, don't return a list, just a single element @@ -106,27 +136,46 @@ def _replace_colons_delete_inner_braces(match: re.Match) -> str: open_brace = dollar_str.find("{") close_brace = dollar_str.rfind("}") - if open_brace != -1 and close_brace != -1 and open_brace < close_brace: + if ( + open_brace != -1 + and close_brace != -1 + and open_brace < close_brace + ): # keep content before the first brace and after the last brace prefix = dollar_str[: open_brace + 1] suffix = dollar_str[close_brace:] # remove braces from the inner content inner_content = dollar_str[open_brace + 1 : close_brace] - inner_content_no_braces = inner_content.replace("{", "").replace("}", "") + inner_content_no_braces = inner_content.replace( + "{", "" + ).replace("}", "") return f"{prefix}{inner_content_no_braces}{suffix}" else: # if we can't find matching outermost braces, return the original string (SHOULD NEVER HAPPEN) return dollar_str - sim_settings_str = re.sub(r"'sensor_names':\{[^\}]*\}", _replace_colons_delete_inner_braces, sim_settings_str) + sim_settings_str = re.sub( + r"'sensor_names':\{[^\}]*\}", + _replace_colons_delete_inner_braces, + sim_settings_str, + ) # replace commas in 'sensor_names':{...} with '&', remove outermost braces def _replace_commas_delete_outer_braces(match: re.Match) -> str: - return match.group(0).replace(",", "&").replace("{", "").replace("}", "") + return ( + match.group(0) + .replace(",", "&") + .replace("{", "") + .replace("}", "") + ) - sim_settings_str = re.sub(r"'sensor_names':\{[^\}]*\}", _replace_commas_delete_outer_braces, sim_settings_str) + sim_settings_str = re.sub( + r"'sensor_names':\{[^\}]*\}", + _replace_commas_delete_outer_braces, + sim_settings_str, + ) # remove single quotes sim_settings_str = sim_settings_str.replace("'", "") @@ -135,20 +184,36 @@ def _replace_commas_delete_outer_braces(match: re.Match) -> str: def _replace_commas_between_curly_braces(match: re.Match) -> str: return match.group(0).replace(",", "|") - sim_settings_str = re.sub(r"\{[^{}]*\}", _replace_commas_between_curly_braces, sim_settings_str) + sim_settings_str = re.sub( + r"\{[^{}]*\}", + _replace_commas_between_curly_braces, + sim_settings_str, + ) # replace commas in 'sensor_settings':[...,...] with "+" def _replace_commas_in_sensor_settings(match: re.Match) -> str: return match.group(0).replace(",", "+") - sim_settings_str = re.sub(r"sensor_settings:\[.*?\]", _replace_commas_in_sensor_settings, sim_settings_str) + sim_settings_str = re.sub( + r"sensor_settings:\[.*?\]", + _replace_commas_in_sensor_settings, + sim_settings_str, + ) # replace colons inside 'sensor_settings':[...:...] with "=" def _replace_colons_in_sensor_settings(match: re.Match) -> str: content = match.group(0) - return re.sub(r"(?<=\[).*?(?=\])", lambda m: m.group(0).replace(":", "="), content) + return re.sub( + r"(?<=\[).*?(?=\])", + lambda m: m.group(0).replace(":", "="), + content, + ) - sim_settings_str = re.sub(r"sensor_settings:\[.*?\]", _replace_colons_in_sensor_settings, sim_settings_str) + sim_settings_str = re.sub( + r"sensor_settings:\[.*?\]", + _replace_colons_in_sensor_settings, + sim_settings_str, + ) # add the formatted string to the dictionary with the ROS parameter as the key sim_settings_dict[sim_settings["ros_parameter"]] = sim_settings_str @@ -170,21 +235,37 @@ def get_parameters_dict(node_settings: Dict) -> Dict: # parse settings for each component if "callback_groups" in node_settings: - callback_group_settings = ",".join([f"{k}:{v}" for k, v in node_settings["callback_groups"].items()]) + callback_group_settings = ",".join( + [f"{k}:{v}" for k, v in node_settings["callback_groups"].items()] + ) else: callback_group_settings = None pub_settings_dict = ( - get_component_settings_subdict(node_settings, "publishers") if "publishers" in node_settings else {} + get_component_settings_subdict(node_settings, "publishers") + if "publishers" in node_settings + else {} ) sub_settings_dict = ( - get_component_settings_subdict(node_settings, "subscribers") if "subscribers" in node_settings else {} + get_component_settings_subdict(node_settings, "subscribers") + if "subscribers" in node_settings + else {} + ) + timer_settings_dict = ( + get_component_settings_subdict(node_settings, "timers") + if "timers" in node_settings + else {} + ) + sim_settings_dict = ( + get_component_sim_settings_subdict(node_settings) + if "sim" in node_settings + else {} ) - timer_settings_dict = get_component_settings_subdict(node_settings, "timers") if "timers" in node_settings else {} - sim_settings_dict = get_component_sim_settings_subdict(node_settings) if "sim" in node_settings else {} # create parameters dictionary for a node by combining all the settings parameters_dict = ( - {"callback_group_settings": callback_group_settings} if callback_group_settings is not None else {} + {"callback_group_settings": callback_group_settings} + if callback_group_settings is not None + else {} ) if "params_path" in node_settings: parameters_dict["params_path"] = node_settings["params_path"] @@ -200,56 +281,75 @@ def get_parameters_dict(node_settings: Dict) -> Dict: def get_launch_actions_from_node_settings( node_settings: Dict, node_type: str, - global_state_node: LifecycleNode, + global_state_node: Optional[LifecycleNode], ) -> List[LifecycleNode]: """Returns the launch actions associated with a node. Parameters: node_settings: the settings dictionary of an Obelisk node. node_type: the type of the node. - global_state_node: the global state node. All Obelisk nodes' lifecycle states match this node's state. + global_state_node: the global state node. All Obelisk nodes' lifecycle + states match this node's state. If `global_state_node` is None, the nodes + will automatically go through the lifecycle. Returns: A list of launch actions. """ assert node_type in ["control", "estimation", "robot", "sensing", "viz"] - def _single_component_launch_actions(node_settings: Dict, suffix: Optional[int] = None) -> List: + def _single_component_launch_actions( + node_settings: Dict, suffix: Optional[int] = None + ) -> List: package = node_settings["pkg"] executable = node_settings["executable"] parameters_dict = get_parameters_dict(node_settings) launch_actions = [] component_node = LifecycleNode( namespace="", - name=f"obelisk_{node_type}" if suffix is None else f"obelisk_{node_type}_{suffix}", + name=( + f"obelisk_{node_type}" + if suffix is None + else f"obelisk_{node_type}_{suffix}" + ), package=package, executable=executable, output="both", parameters=[parameters_dict], ) launch_actions += [component_node] - launch_actions += get_handlers(component_node, global_state_node) + launch_actions += get_handlers( + component_node, global_state_node=global_state_node + ) return launch_actions node_launch_actions = [] for i, node_setting in enumerate(node_settings): - node_launch_actions += _single_component_launch_actions(node_setting, suffix=i) + node_launch_actions += _single_component_launch_actions( + node_setting, suffix=i + ) return node_launch_actions -def get_launch_actions_from_viz_settings(settings: Dict, global_state_node: LifecycleNode) -> List[LifecycleNode]: +def get_launch_actions_from_viz_settings( + settings: Dict, global_state_node: Optional[LifecycleNode] +) -> List[LifecycleNode]: """Gets and configures all the launch actions related to viz given the settings from the yaml.""" launch_actions = [] if settings["on"]: - def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None) -> List: + def _single_viz_node_launch_actions( + settings: Dict, suffix: Optional[int] = None + ) -> List: package = settings["pkg"] executable = settings["executable"] parameters_dict = get_parameters_dict(settings) # Get the other viz specific params urdf_file_name = "urdf/" + settings["urdf"] - urdf_path = os.path.join(get_package_share_directory(settings["robot_pkg"]), urdf_file_name) + urdf_path = os.path.join( + get_package_share_directory(settings["robot_pkg"]), + urdf_file_name, + ) parameters_dict["urdf_path_param"] = urdf_path if "tf_prefix" in settings: tf_prefix = settings["tf_prefix"] + "/" @@ -260,14 +360,18 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None launch_actions = [] component_node = LifecycleNode( namespace="", - name="obelisk_viz" if suffix is None else f"obelisk_viz_{suffix}", + name="obelisk_viz" + if suffix is None + else f"obelisk_viz_{suffix}", package=package, executable=executable, output="both", parameters=[parameters_dict], ) launch_actions += [component_node] - launch_actions += get_handlers(component_node, global_state_node) + launch_actions += get_handlers( + component_node, global_state_node=global_state_node + ) # Read the URDF for the robot_state_publisher with open(urdf_path, "r") as urdf: @@ -292,7 +396,8 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None "use_sim_time": False, "robot_description": robot_desc, "frame_prefix": tf_prefix, - "publish_frequency": 1.0 / timer_settings[0]["timer_period_sec"], + "publish_frequency": 1.0 + / timer_settings[0]["timer_period_sec"], } ], remappings=[ @@ -313,12 +418,17 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None # Get the launch actions for each ObeliskViz node node_settings = settings["viz_nodes"] for i, viz_node_settings in enumerate(node_settings): - launch_actions += _single_viz_node_launch_actions(viz_node_settings, suffix=i) + launch_actions += _single_viz_node_launch_actions( + viz_node_settings, suffix=i + ) if "viz_tool" not in settings or settings["viz_tool"] == "rviz": # Setup Rviz rviz_file_name = "rviz/" + settings["rviz_config"] - rviz_config_path = os.path.join(get_package_share_directory(settings["rviz_pkg"]), rviz_file_name) + rviz_config_path = os.path.join( + get_package_share_directory(settings["rviz_pkg"]), + rviz_file_name, + ) launch_actions += [ Node( package="rviz2", @@ -331,7 +441,9 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None elif settings["viz_tool"] == "foxglove": # setup fox glove xml_launch_file = os.path.join( - get_package_share_directory("foxglove_bridge"), "launch", "foxglove_bridge_launch.xml" + get_package_share_directory("foxglove_bridge"), + "launch", + "foxglove_bridge_launch.xml", ) launch_actions += [ IncludeLaunchDescription( @@ -342,12 +454,16 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None ) ] else: - raise RuntimeError("Invalid setting for `viz_tool`. Must be either `rviz` of `foxglove`.") + raise RuntimeError( + "Invalid setting for `viz_tool`. Must be either `rviz` of `foxglove`." + ) return launch_actions -def get_launch_actions_from_joystick_settings(settings: Dict, global_state_node: LifecycleNode) -> List[LifecycleNode]: +def get_launch_actions_from_joystick_settings( + settings: Dict, +) -> List[LifecycleNode]: """Gets and configures all the launch actions related to joystick given the settings from the yaml.""" launch_actions = [] if settings["on"]: @@ -355,19 +471,37 @@ def get_launch_actions_from_joystick_settings(settings: Dict, global_state_node: # is supplied. device_id = settings["device_id"] if "device_id" in settings else 0 - device_name = settings["device_name"] if "device_name" in settings else "" + device_name = ( + settings["device_name"] if "device_name" in settings else "" + ) deadzone = settings["deadzone"] if "deadzone" in settings else 0.05 - autorepeat_rate = settings["autorepeat_rate"] if "autorepeat_rate" in settings else 20.0 + autorepeat_rate = ( + settings["autorepeat_rate"] + if "autorepeat_rate" in settings + else 20.0 + ) - sticky_buttons = settings["sticky_buttons"] if "sticky_buttons" in settings else False + sticky_buttons = ( + settings["sticky_buttons"] + if "sticky_buttons" in settings + else False + ) - coalesce_interval_ms = settings["coalesce_interval_ms"] if "coalesce_interval_ms" in settings else 1 + coalesce_interval_ms = ( + settings["coalesce_interval_ms"] + if "coalesce_interval_ms" in settings + else 1 + ) pub_topic = settings["pub_topic"] if "pub_topic" in settings else "/joy" - sub_topic = settings["sub_topic"] if "sub_topic" in settings else "/joy/set_feedback" + sub_topic = ( + settings["sub_topic"] + if "sub_topic" in settings + else "/joy/set_feedback" + ) launch_actions += [ Node( @@ -401,119 +535,83 @@ def get_launch_actions_from_joystick_settings(settings: Dict, global_state_node: return launch_actions -def get_handlers(component_node: LifecycleNode, global_state_node: LifecycleNode) -> List: +def get_handlers( + component_node: LifecycleNode, global_state_node: Optional[LifecycleNode] +) -> List: """Gets all the handlers for the Lifecycle node.""" - # transition events to match the global state node - configure_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=launch.events.matches_action(component_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - activate_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=launch.events.matches_action(component_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - ) - ) - deactivate_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=launch.events.matches_action(component_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_DEACTIVATE, - ) - ) - cleanup_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=launch.events.matches_action(component_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP, - ) - ) - unconfigured_shutdown_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=launch.events.matches_action(component_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_UNCONFIGURED_SHUTDOWN, - ) - ) - inactive_shutdown_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=launch.events.matches_action(component_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_INACTIVE_SHUTDOWN, + lifecycle_node_matcher = matches_action(component_node) + + # Define transition events with their IDs + transitions = { + "configure": Transition.TRANSITION_CONFIGURE, + "activate": Transition.TRANSITION_ACTIVATE, + "deactivate": Transition.TRANSITION_DEACTIVATE, + "cleanup": Transition.TRANSITION_CLEANUP, + "unconfigured_shutdown": Transition.TRANSITION_UNCONFIGURED_SHUTDOWN, + "inactive_shutdown": Transition.TRANSITION_INACTIVE_SHUTDOWN, + "active_shutdown": Transition.TRANSITION_ACTIVE_SHUTDOWN, + } + + events = { + name: EmitEvent( + event=ChangeState( + lifecycle_node_matcher=lifecycle_node_matcher, + transition_id=transition_id, + ) ) - ) - active_shutdown_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=launch.events.matches_action(component_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN, + for name, transition_id in transitions.items() + } + + def make_handler( + start: str, goal: str, event_name: str, target: LifecycleNode + ) -> RegisterEventHandler: + """ + When the `target_lifecycle_node` transitions from `start` to `goal`, + emit the event `event_name` to the component node. + """ # noqa: D205 + return RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=target, + start_state=start, + goal_state=goal, + entities=[events[event_name]], + ) ) - ) - # making event handlers - configure_handler = RegisterEventHandler( - launch_ros.event_handlers.on_state_transition.OnStateTransition( - target_lifecycle_node=global_state_node, - start_state="configuring", - goal_state="inactive", - entities=[configure_event], - ) - ) - activate_handler = RegisterEventHandler( - launch_ros.event_handlers.on_state_transition.OnStateTransition( - target_lifecycle_node=global_state_node, - start_state="activating", - goal_state="active", - entities=[activate_event], - ) - ) - deactivate_handler = RegisterEventHandler( - launch_ros.event_handlers.on_state_transition.OnStateTransition( - target_lifecycle_node=global_state_node, - start_state="deactivating", - goal_state="inactive", - entities=[deactivate_event], - ) - ) - cleanup_handler = RegisterEventHandler( - launch_ros.event_handlers.on_state_transition.OnStateTransition( - target_lifecycle_node=global_state_node, - start_state="cleaningup", - goal_state="unconfigured", - entities=[cleanup_event], - ) - ) - unconfigured_shutdown_handler = RegisterEventHandler( - launch_ros.event_handlers.on_state_transition.OnStateTransition( - target_lifecycle_node=global_state_node, - start_state="unconfigured", - goal_state="shuttingdown", - entities=[unconfigured_shutdown_event], - ) - ) - inactive_shutdown_handler = RegisterEventHandler( - launch_ros.event_handlers.on_state_transition.OnStateTransition( - target_lifecycle_node=global_state_node, - start_state="inactive", - goal_state="shuttingdown", - entities=[inactive_shutdown_event], - ) - ) - active_shutdown_handler = RegisterEventHandler( - launch_ros.event_handlers.on_state_transition.OnStateTransition( - target_lifecycle_node=global_state_node, - start_state="active", - goal_state="shuttingdown", - entities=[active_shutdown_event], - ) - ) - launch_actions = [ - configure_handler, - activate_handler, - deactivate_handler, - cleanup_handler, - unconfigured_shutdown_handler, - inactive_shutdown_handler, - active_shutdown_handler, + # Event handlers for the component node depend solely on the state of the + # component node + if not global_state_node: + target = component_node + handlers = [ + RegisterEventHandler( + OnProcessStart( + target_action=target, + on_start=[events["configure"]], + ) + ), + make_handler("configuring", "inactive", "activate", target), + make_handler("active", "deactivating", "deactivate", target), + make_handler("inactive", "cleaningup", "cleanup", target), + ] + else: + # Event handlers will transition the component node's state to match + # that of the global state node + target = global_state_node + handlers = [ + make_handler("configuring", "inactive", "configure", target), + make_handler("activating", "active", "activate", target), + make_handler("deactivating", "inactive", "deactivate", target), + make_handler("cleaningup", "unconfigured", "cleanup", target), + ] + # Add shutting down handlers + handlers += [ + make_handler( + "unconfigured", "shuttingdown", "unconfigured_shutdown", target + ), + make_handler("inactive", "shuttingdown", "inactive_shutdown", target), + make_handler("active", "shuttingdown", "active_shutdown", target), ] - return launch_actions + return handlers def setup_logging_dir(config_name: str) -> str: @@ -541,7 +639,9 @@ def setup_logging_dir(config_name: str) -> str: curr_date_time = now.strftime("%Y%m%d_%H%M%S") # Now make a folder for this specific run - run_log_file_path = general_log_file_path + "/" + config_name + "_" + curr_date_time + run_log_file_path = ( + general_log_file_path + "/" + config_name + "_" + curr_date_time + ) os.makedirs(run_log_file_path) # Set the ROS environment variable diff --git a/obelisk/python/obelisk_py/core/utils/msg.py b/obelisk/python/obelisk_py/core/utils/msg.py index eb163a32..03e47d14 100644 --- a/obelisk/python/obelisk_py/core/utils/msg.py +++ b/obelisk/python/obelisk_py/core/utils/msg.py @@ -34,7 +34,9 @@ def multiarray_to_np(msg: MultiArray) -> np.ndarray: shape = [dim.size for dim in msg.layout.dim] strides = [dim.stride * flat_data.itemsize for dim in msg.layout.dim] - return np.lib.stride_tricks.as_strided(flat_data, shape=shape, strides=strides) + return np.lib.stride_tricks.as_strided( + flat_data, shape=shape, strides=strides + ) def np_to_multiarray(arr: np.ndarray) -> MultiArray: @@ -66,7 +68,9 @@ def np_to_multiarray(arr: np.ndarray) -> MultiArray: for i, (size, stride) in enumerate(zip(arr.shape, arr.strides)): dim = MultiArrayDimension() dim.size = size - dim.stride = stride // arr.itemsize # convert byte strides to element strides + dim.stride = ( + stride // arr.itemsize + ) # convert byte strides to element strides dim.label = f"dim_{i}" dimensions.append(dim) diff --git a/obelisk/python/obelisk_py/core/utils/ros.py b/obelisk/python/obelisk_py/core/utils/ros.py index a115f4cb..910102ac 100644 --- a/obelisk/python/obelisk_py/core/utils/ros.py +++ b/obelisk/python/obelisk_py/core/utils/ros.py @@ -1,7 +1,11 @@ -from typing import List, Optional, Type, Union +from typing import List, Optional, Type, Union # noqa: I001 import rclpy -from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.executors import ( + ExternalShutdownException, + MultiThreadedExecutor, + SingleThreadedExecutor, +) from obelisk_py.core.node import ObeliskNode @@ -9,7 +13,10 @@ def spin_obelisk( args: Optional[List], node_type: Type[ObeliskNode], - executor_type: Union[Type[SingleThreadedExecutor], Type[MultiThreadedExecutor]], + executor_type: Union[ + Type[SingleThreadedExecutor], Type[MultiThreadedExecutor] + ], + node_name: str = "obelisk_node", node_kwargs: Optional[dict] = None, ) -> None: """Spin an Obelisk node. @@ -20,8 +27,10 @@ def spin_obelisk( executor_type: Executor type to use. node_kwargs: Keyword arguments to pass to the node """ - rclpy.init(args=args) - node = node_type(node_name="obelisk_node", **(node_kwargs or {})) + # rclpy may already be initialized when the global_state node has been launched + if not rclpy.ok(): # check if rclpy is not initialized + rclpy.init(args=args) + node = node_type(node_name=node_name, **(node_kwargs or {})) executor = executor_type() executor.add_node(node) try: @@ -29,5 +38,7 @@ def spin_obelisk( except (KeyboardInterrupt, ExternalShutdownException): pass finally: + executor.remove_node(node) node.destroy_node() - rclpy.shutdown() + if rclpy.ok(): # Only shutdown if context is still valid + rclpy.shutdown() diff --git a/obelisk/python/obelisk_py/zoo/control/example/example_position_setpoint_controller.py b/obelisk/python/obelisk_py/zoo/control/example/example_position_setpoint_controller.py index 365b3e3e..64d284e7 100644 --- a/obelisk/python/obelisk_py/zoo/control/example/example_position_setpoint_controller.py +++ b/obelisk/python/obelisk_py/zoo/control/example/example_position_setpoint_controller.py @@ -12,14 +12,18 @@ class ExamplePositionSetpointController(ObeliskController): """Example position setpoint controller.""" - def __init__(self, node_name: str = "example_position_setpoint_controller") -> None: + def __init__( + self, node_name: str = "example_position_setpoint_controller" + ) -> None: """Initialize the example position setpoint controller.""" super().__init__(node_name, PositionSetpoint, EstimatedState) self.declare_parameter("test_param", "default_value") - self.get_logger().info(f"test_param: {self.get_parameter('test_param').get_parameter_value().string_value}") + self.get_logger().info( + f"test_param: {self.get_parameter('test_param').get_parameter_value().string_value}" + ) def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: - """Configure the controller.""" + """Configure the controller. I.e. Declare all variables required to compute the control input.""" super().on_configure(state) self.joint_pos = None return TransitionCallbackReturn.SUCCESS diff --git a/obelisk/python/obelisk_py/zoo/control/example/leap_example_pos_setpoint_controller.py b/obelisk/python/obelisk_py/zoo/control/example/leap_example_pos_setpoint_controller.py index ba9bb6d8..9e151cbd 100644 --- a/obelisk/python/obelisk_py/zoo/control/example/leap_example_pos_setpoint_controller.py +++ b/obelisk/python/obelisk_py/zoo/control/example/leap_example_pos_setpoint_controller.py @@ -12,7 +12,9 @@ class LeapExamplePositionSetpointController(ObeliskController): """Example position setpoint controller.""" - def __init__(self, node_name: str = "leap_example_position_setpoint_controller") -> None: + def __init__( + self, node_name: str = "leap_example_position_setpoint_controller" + ) -> None: """Initialize the example position setpoint controller.""" super().__init__(node_name, PositionSetpoint, EstimatedState) @@ -38,8 +40,12 @@ def compute_control(self) -> Type: """ # setting the message position_setpoint_msg = PositionSetpoint() - position_setpoint_msg.u_mujoco = [(0.3 * np.sin(self.t)) for _ in range(16)] # example state-independent input - position_setpoint_msg.q_des = [(0.3 * np.sin(self.t)) for _ in range(16)] # example state-independent input + position_setpoint_msg.u_mujoco = [ + (0.3 * np.sin(self.t)) for _ in range(16) + ] # example state-independent input + position_setpoint_msg.q_des = [ + (0.3 * np.sin(self.t)) for _ in range(16) + ] # example state-independent input self.obk_publishers["pub_ctrl"].publish(position_setpoint_msg) assert is_in_bound(type(position_setpoint_msg), ObeliskControlMsg) return position_setpoint_msg # type: ignore diff --git a/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py b/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py index ebc11705..be1ba4dc 100644 --- a/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py +++ b/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py @@ -10,7 +10,9 @@ class JointEncodersPassthroughEstimator(ObeliskEstimator): """Passthrough estimator for joint encoder sensors.""" - def __init__(self, node_name: str = "joint_encoders_passthrough_estimator") -> None: + def __init__( + self, node_name: str = "joint_encoders_passthrough_estimator" + ) -> None: """Initialize the joint encoders passthrough estimator.""" super().__init__(node_name, EstimatedState) self.register_obk_subscription( diff --git a/obelisk/python/pyproject.toml b/obelisk/python/pyproject.toml index 5bcc6f60..97a278b5 100644 --- a/obelisk/python/pyproject.toml +++ b/obelisk/python/pyproject.toml @@ -17,3 +17,6 @@ dependencies = [] [tool.hatch.build.targets.wheel] include = ["obelisk_py*"] + +[tool.ruff] +line-length = 80 diff --git a/obelisk_ws/src/cpp/hardware/obelisk_unitree_cpp/CMakeLists.txt b/obelisk_ws/src/cpp/hardware/obelisk_unitree_cpp/CMakeLists.txt index 90899ee1..404eac30 100644 --- a/obelisk_ws/src/cpp/hardware/obelisk_unitree_cpp/CMakeLists.txt +++ b/obelisk_ws/src/cpp/hardware/obelisk_unitree_cpp/CMakeLists.txt @@ -97,4 +97,3 @@ if(DEFINED ENV{OBELISK_BUILD_UNITREE} AND "$ENV{OBELISK_BUILD_UNITREE}" STREQUAL ament_package() endif() - diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index bac0fdb9..0a4d4a11 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -68,7 +68,7 @@ onboard: callback_group: None # sensing: robot: - # # === simulation === + # === simulation === # - is_simulated: True # pkg: obelisk_unitree_cpp # executable: obelisk_unitree_sim @@ -249,31 +249,31 @@ onboard: - ros_parameter: timer_sensor_setting timer_period_sec: 0.02 callback_group: None - viz: - on: True - viz_tool: foxglove - viz_nodes: - - pkg: obelisk_viz_cpp - executable: default_robot_viz - robot_pkg: g1_description - urdf: g1.urdf - robot_topic: robot_description - subscribers: - - ros_parameter: sub_viz_est_setting - topic: /obelisk/g1/est_state - history_depth: 10 - callback_group: None - non_obelisk: False - publishers: - - ros_parameter: pub_viz_joint_setting - topic: joint_states - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_viz_joint_setting - timer_period_sec: 0.01 - callback_group: None - joystick: - on: True - pub_topic: /obelisk/g1/joy - sub_topic: /obelisk/g1/joy_feedback \ No newline at end of file + # viz: + # on: True + # viz_tool: foxglove + # viz_nodes: + # - pkg: obelisk_viz_cpp + # executable: default_robot_viz + # robot_pkg: g1_description + # urdf: g1.urdf + # robot_topic: robot_description + # subscribers: + # - ros_parameter: sub_viz_est_setting + # topic: /obelisk/g1/est_state + # history_depth: 10 + # callback_group: None + # non_obelisk: False + # publishers: + # - ros_parameter: pub_viz_joint_setting + # topic: joint_states + # history_depth: 10 + # callback_group: None + # timers: + # - ros_parameter: timer_viz_joint_setting + # timer_period_sec: 0.01 + # callback_group: None + # joystick: + # on: True + # pub_topic: /obelisk/g1/joy + # sub_topic: /obelisk/g1/joy_feedback \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml index 31287381..2fc5375c 100644 --- a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml @@ -75,29 +75,29 @@ onboard: # sensing: robot: # === simulation === - # - is_simulated: True - # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_sim - # params: - # ic_keyframe: standing - # === hardware === - - is_simulated: False + - is_simulated: True pkg: obelisk_unitree_cpp - executable: obelisk_unitree_go2_hardware + executable: obelisk_unitree_sim params: - network_interface_name: enp4s0 #enx001cc24ce09a - default_kp: [ - 60., 60., 60., # FR - 60., 60., 60., # FL - 60., 60., 60., # RR - 60., 60., 60., # RL - ] - default_kd: [ - 5., 5., 5., # FR - 5., 5., 5., # FL - 5., 5., 5., # RR - 5., 5., 5., # RL - ] + ic_keyframe: standing + # === hardware === + # - is_simulated: False + # pkg: obelisk_unitree_cpp + # executable: obelisk_unitree_go2_hardware + # params: + # network_interface_name: enp4s0 #enx001cc24ce09a + # default_kp: [ + # 60., 60., 60., # FR + # 60., 60., 60., # FL + # 60., 60., 60., # RR + # 60., 60., 60., # RL + # ] + # default_kd: [ + # 5., 5., 5., # FR + # 5., 5., 5., # FL + # 5., 5., 5., # RR + # 5., 5., 5., # RL + # ] # ================== # callback_groups: publishers: diff --git a/obelisk_ws/src/obelisk_ros/config/leap_py.yaml b/obelisk_ws/src/obelisk_ros/config/leap_py.yaml index 9167bac9..14801af1 100644 --- a/obelisk_ws/src/obelisk_ros/config/leap_py.yaml +++ b/obelisk_ws/src/obelisk_ros/config/leap_py.yaml @@ -91,4 +91,4 @@ onboard: timers: - ros_parameter: timer_sensor_setting timer_period_sec: 0.02 - callback_group: None + callback_group: None \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py b/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py index 853e4954..1c1a69eb 100644 --- a/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py +++ b/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py @@ -4,7 +4,14 @@ import launch_ros import lifecycle_msgs.msg from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, EmitEvent, ExecuteProcess, OpaqueFunction, RegisterEventHandler +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + ExecuteProcess, + OpaqueFunction, + RegisterEventHandler, + LogInfo, +) from launch.substitutions import LaunchConfiguration from launch_ros.actions import LifecycleNode from launch_ros.events.lifecycle import ChangeState @@ -100,30 +107,47 @@ def obelisk_setup(context: launch.LaunchContext, launch_args: Dict) -> List: # If auto_start is "configure" then only configure the nodes # If auto_start is anything else, then no configuration or activation if auto_start in ["true", "activate"]: - # Configure and activate all nodes + # ignore the global_state_node + global_state_node = None + """ + # Configure all nodes + logger.info("Configure event") configure_event = EmitEvent( event=ChangeState( lifecycle_node_matcher=launch.events.matches_action(global_state_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, # 1 ) ) + obelisk_launch_actions += [configure_event] + + # Add handlers to activate all nodes and properly change their states + logger.info("Activate event") activate_event = EmitEvent( event=ChangeState( lifecycle_node_matcher=launch.events.matches_action(global_state_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, # 3 ) ) + logger.info("activate_upon_configure_handler") activate_upon_configure_handler = RegisterEventHandler( launch_ros.event_handlers.on_state_transition.OnStateTransition( target_lifecycle_node=global_state_node, start_state="configuring", goal_state="inactive", - entities=[activate_event], + entities=[activate_event, + LogInfo(msg="on_configure succeeded - activating")], ) - ) # once the node is configured, it will be activated automatically - obelisk_launch_actions += [configure_event, activate_upon_configure_handler] + ) # once the global_state_node is configured, it will be activated automatically. + # This will trigger the event handlers to activate all the other nodes. + # BUG: If the global state node does this before one of the component nodes + # is configured, we get a "transition is not registered" error. + logger.info("obelisk_launch_actions") + # obelisk_launch_actions += [configure_event, + # activate_upon_configure_handler, + # ] + """ elif auto_start == "configure": - # Just configure all nodes + # Configure all nodes configure_event = EmitEvent( event=ChangeState( lifecycle_node_matcher=launch.events.matches_action(global_state_node), @@ -161,7 +185,7 @@ def obelisk_setup(context: launch.LaunchContext, launch_args: Dict) -> List: if "joystick" in obelisk_config: logger.info("joystick present in config file.") obelisk_launch_actions += get_launch_actions_from_joystick_settings( - obelisk_config["joystick"], global_state_node + obelisk_config["joystick"], ) return obelisk_launch_actions diff --git a/obelisk_ws/src/obelisk_ros/obelisk_ros/global_state.py b/obelisk_ws/src/obelisk_ros/obelisk_ros/global_state.py index 0fc7ff38..a957bc51 100644 --- a/obelisk_ws/src/obelisk_ros/obelisk_ros/global_state.py +++ b/obelisk_ws/src/obelisk_ros/obelisk_ros/global_state.py @@ -4,6 +4,8 @@ from rclpy.executors import SingleThreadedExecutor from rclpy.lifecycle import LifecycleNode +from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor, SingleThreadedExecutor + class GlobalStateNode(LifecycleNode): """A dummy class whose only purpose is to store the global state of the system. @@ -19,13 +21,21 @@ def __init__(self) -> None: def main(args: Optional[List] = None) -> None: """Main entrypoint.""" - rclpy.init(args=args) - obelisk_mujoco_robot = GlobalStateNode() + # rclpy may already be initialized when the other nodes have been launched + if not rclpy.ok(): # check if rclpy is not initialized + rclpy.init(args=args) + node = GlobalStateNode() executor = SingleThreadedExecutor() - executor.add_node(obelisk_mujoco_robot) - executor.spin() - obelisk_mujoco_robot.destroy_node() - rclpy.shutdown() + executor.add_node(node) + try: + executor.spin() + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + executor.remove_node(node) + node.destroy_node() + if rclpy.ok(): # Only shutdown if context is still valid + rclpy.shutdown() if __name__ == "__main__": diff --git a/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/__init__.py b/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/obelisk_ws/src/python/obelisk_sim_py/obelisk_sim_py/obelisk_mujoco_robot.py b/obelisk_ws/src/python/obelisk_sim_py/obelisk_sim_py/obelisk_mujoco_robot.py index 126cecf8..b0805274 100644 --- a/obelisk_ws/src/python/obelisk_sim_py/obelisk_sim_py/obelisk_mujoco_robot.py +++ b/obelisk_ws/src/python/obelisk_sim_py/obelisk_sim_py/obelisk_mujoco_robot.py @@ -10,7 +10,7 @@ def main(args: Optional[List] = None) -> None: """Main entrypoint.""" ctrl_msg_type = PositionSetpoint - spin_obelisk(args, ObeliskMujocoRobot, MultiThreadedExecutor, {"ctrl_msg_type": ctrl_msg_type}) + spin_obelisk(args, ObeliskMujocoRobot, MultiThreadedExecutor, node_kwargs={"ctrl_msg_type": ctrl_msg_type}) if __name__ == "__main__": diff --git a/scripts/docker_setup.sh b/scripts/docker_setup.sh index 06500904..98fdfdc0 100644 --- a/scripts/docker_setup.sh +++ b/scripts/docker_setup.sh @@ -214,3 +214,7 @@ fi cp $OBELISK_ROOT/scripts/install_sys_deps.sh $OBELISK_ROOT/docker/install_sys_deps.sh cp $OBELISK_ROOT/scripts/config_groups.sh $OBELISK_ROOT/docker/config_groups.sh cp $OBELISK_ROOT/scripts/user_setup.sh $OBELISK_ROOT/docker/user_setup.sh + +# Copy obelisk/python directory into docker directory +mkdir -p $OBELISK_ROOT/docker/obelisk +cp -r $OBELISK_ROOT/obelisk/python $OBELISK_ROOT/docker/obelisk diff --git a/scripts/install_sys_deps.sh b/scripts/install_sys_deps.sh index e6fa4c7c..84f42a0a 100644 --- a/scripts/install_sys_deps.sh +++ b/scripts/install_sys_deps.sh @@ -96,13 +96,9 @@ if [ "$basic" = true ]; then colcon-common-extensions \ "ruamel.yaml" \ mujoco - if [ -d $OBELISK_ROOT ]; then - pip install -e $OBELISK_ROOT/obelisk/python - echo -e "\033[1;32mOBELISK_ROOT exists, obelisk_py installed as editable!\033[0m" - else - pip install git+https://github.com/Caltech-AMBER/obelisk.git#subdirectory=obelisk/python - echo -e "\033[1;33mOBELISK_ROOT directory does not exist! Installing obelisk_py from GitHub...\033[0m" - fi + + # Install an uneditable version of obelisk_py + pip install git+https://github.com/Caltech-AMBER/obelisk.git#subdirectory=obelisk/python echo -e "\033[1;32mSystem dependencies installed successfully!\033[0m" else