D1 arm

README.md

@@ -115,5 +115,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_file_path=<config file> device_name=<device>
+```
+
+For the dummy examples this looks like:
+```
+obk-launch config_file_path=dummy_cpp.yaml device_name=onboard
+```
+
 ## Building Docs
 In the repository root, to build the docs locally, run `sphinx-build -M html docs/source/ docs/build/`.

docs/source/obelisk_terminal_aliases.md

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

obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h

@@ -42,15 +42,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") {
@@ -84,7 +88,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;
         }
 
@@ -95,10 +99,11 @@ namespace obelisk {
          */
         rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
         on_activate(const rclcpp_lifecycle::State& prev_state) {
+            RCLCPP_INFO_STREAM(this->get_logger(), "Activating the ObeliskSimRobot robot");
             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;
         }
 
@@ -180,7 +185,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!");
             }
@@ -210,9 +214,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();
                 {
@@ -222,9 +227,11 @@ namespace obelisk {
                         data_->ctrl[i] = shared_data_.at(i);
                     }
                 }
-
+                
                 {
+                    // RCLCPP_INFO_STREAM(this->get_logger(), "1 shared_data_.size()" << shared_data_.size());
                     std::lock_guard<std::mutex> lock(sensor_data_mut_);
+                    // RCLCPP_INFO_STREAM(this->get_logger(), "2 shared_data_.size()" << shared_data_.size());
                     mj_step(model_, data_);
                 }
 
@@ -233,12 +240,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/cpp/zoo/hardware/robots/unitree/CMakeLists.txt

@@ -6,27 +6,70 @@ 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 7b00ca4b766272d137cf7ff751d4a067bc99ca85) # Most recent commit includes the Write()
+    # member function in channel_publisher.hpp
+
+    # GIT_TAG 3a4680ae9b00df59e60f7e63cfb0fcc432a9d08d # This commit doesn't include the aforementioned
+    # member function, however, it has the SwitchGait() member function which enables go2_interface.h to compile.
+
+# 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)
 
+# Get the .cpp files in msg and store it into the `UNITREE_ARM_MSGS`. A library should include
+# solely .cpp files.
+set(UNITREE_ARM_MSGS
+  d1/msg/ArmString_.cpp
+)
+
+# Creates a library called `unitree_arm_msgs` containing all files in `UNITREE_ARM_MSGS`
+add_library(unitree_arm_msgs SHARED ${UNITREE_ARM_MSGS})
+
+target_include_directories(unitree_arm_msgs PUBLIC
+  ${CMAKE_CURRENT_SOURCE_DIR}/msg # The compiler needs ${CMAKE_CURRENT_SOURCE_DIR}/msg 
+  # in the include path to find ArmString_.hpp when compiling ArmString_.cpp
+  /usr/local/include/ddscxx 
+  /usr/local/include/iceoryx/v2.0.2 # FIXME: not sure if this is necessary. 
+  # It's in d1_sdk's CMakeLists.txt, but I can't find the folder on the computer.
+)
+
+# Link `unitree_arm_msgs` to other libraries that d1_sdk depends on. These other libraries
+# are listed in d1_sdk's CMakeLists.txt.
+target_link_libraries(unitree_arm_msgs PUBLIC
+  unitree_sdk2 ddsc ddscxx rt pthread
+)
+
+# Adds an Object Library to compile source files without linking their 
+# object files into a library
 add_library(UnitreeInterface INTERFACE)
-target_include_directories(UnitreeInterface INTERFACE .)
 
-target_link_libraries(UnitreeInterface INTERFACE Obelisk::Core unitree_sdk2)
+# Adds the current directory (.) to the include path for the UnitreeInterface library
+target_include_directories(UnitreeInterface INTERFACE .
+${CMAKE_CURRENT_SOURCE_DIR}/msg
+)
 
+# 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 
+  unitree_arm_msgs
+)
+
+# 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/zoo/hardware/robots/unitree/d1/msg/ArmString_.cpp

@@ -0,0 +1,53 @@
+/****************************************************************
+
+  Generated by Eclipse Cyclone DDS IDL to CXX Translator
+  File name: ArmString_.idl
+  Source: ArmString_.cpp
+  Cyclone DDS: v0.10.2
+
+*****************************************************************/
+#include "ArmString_.hpp"
+
+namespace org{
+namespace eclipse{
+namespace cyclonedds{
+namespace core{
+namespace cdr{
+
+template<>
+propvec &get_type_props<::unitree_arm::msg::dds_::ArmString_>() {
+  static thread_local std::mutex mtx;
+  static thread_local propvec props;
+  static thread_local entity_properties_t *props_end = nullptr;
+  static thread_local std::atomic_bool initialized {false};
+  key_endpoint keylist;
+  if (initialized.load(std::memory_order_relaxed)) {
+    auto ptr = props.data();
+    while (ptr < props_end)
+      (ptr++)->is_present = false;
+    return props;
+  }
+  std::lock_guard<std::mutex> lock(mtx);
+  if (initialized.load(std::memory_order_relaxed)) {
+    auto ptr = props.data();
+    while (ptr < props_end)
+      (ptr++)->is_present = false;
+    return props;
+  }
+  props.clear();
+
+  props.push_back(entity_properties_t(0, 0, false, bb_unset, extensibility::ext_final));  //root
+  props.push_back(entity_properties_t(1, 0, false, bb_unset, extensibility::ext_final, false));  //::data_
+
+  entity_properties_t::finish(props, keylist);
+  props_end = props.data() + props.size();
+  initialized.store(true, std::memory_order_release);
+  return props;
+}
+
+} //namespace cdr
+} //namespace core
+} //namespace cyclonedds
+} //namespace eclipse
+} //namespace org
+