Added documentation and bug fixes during SURF 2025 (excluding D1 Arm code)

.gitignore

@@ -246,3 +246,4 @@ MUJOCO_LOG.TXT
 obk_logs/
 docker/user_setup.sh
 docker/install_sys_deps.sh
+docker/obelisk

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=<config file> device=<name> auto_start=<True|False>
+```
+
+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/`.

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=""; \

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
 ```
 

docs/source/obelisk_terminal_aliases.md

@@ -31,6 +31,7 @@ Configure all Obelisk nodes.
 ```
 obk-configure <config_name>
 ```
+For example, the config_name is `dummy` for dummy_cpp.yaml.
 
 ### `obk-activate`
 Activate all Obelisk nodes.

obelisk/cpp/hardware/include/unitree_example_controller.h

@@ -80,7 +80,8 @@ namespace obelisk {
             }
 
             this->GetPublisher<unitree_controller_msg>(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;
         };
 

obelisk/cpp/hardware/include/unitree_example_estimator.h

@@ -9,34 +9,57 @@ namespace obelisk {
     public:
         UnitreeExampleEstimator(const std::string& name)
             : obelisk::ObeliskEstimator<obelisk_estimator_msgs::msg::EstimatedState>(name) {
-
             this->RegisterObkSubscription<obelisk_sensor_msgs::msg::ObkJointEncoders>(
                 "sub_sensor_setting", "sub_sensor",
                 std::bind(&UnitreeExampleEstimator::JointEncoderCallback, this, std::placeholders::_1));
         }
 
     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<obelisk_estimator_msgs::msg::EstimatedState>(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<obelisk_estimator_msgs::msg::EstimatedState>(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<double> joint_encoders_;
         std::vector<double> joint_vels_;
         std::vector<std::string> joint_names_;

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<obelisk_estimator_msgs::msg::EstimatedState>(this->est_pub_key_)->publish(msg);
 

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)
 

obelisk/cpp/hardware/robots/unitree/go2_interface.h

@@ -55,15 +55,15 @@ namespace obelisk {
         }
     protected:
         void CreateUnitreePublishers() override {
-            // create low level command publisher
+            // Create low level command publisher
             lowcmd_publisher_.reset(new ChannelPublisher<LowCmd_>(CMD_TOPIC_));
             lowcmd_publisher_->InitChannel();
 
             RCLCPP_INFO_STREAM(this->get_logger(), "Go2 command publishers created!");
         }
 
         void CreateUnitreeSubscribers() override {
-            // Dreate unitree subscriber
+            // Create unitree subscriber
             // Need to create after the publishers have been activated
             lowstate_subscriber_.reset(new ChannelSubscriber<LowState_>(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_> lowcmd_publisher_;
+        ChannelPublisherPtr<LowCmd_> lowcmd_publisher_;
         ChannelSubscriberPtr<LowState_> lowstate_subscriber_;
         // ChannelSubscriberPtr<IMUState_> odom_subscriber_;
 

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<ControlMessageT>::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<ControlMessageT>::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<std::mutex> 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();
         }
 

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(

obelisk/cpp/obelisk_cpp/include/obelisk_ros_utils.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <string>
+#include <stdexcept>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -15,14 +16,32 @@ namespace obelisk::utils {
      * @param name the name to give to the node
      */
     template <typename NodeT, typename ExecutorT> 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<NodeT>(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

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(