Feb hot fix

.claude/settings.local.json

@@ -0,0 +1,7 @@
+{
+  "permissions": {
+    "allow": [
+      "Read(//home/wcompton/repos/IsaacLab/source/isaaclab/isaaclab/sensors/ray_caster/patterns/**)"
+    ]
+  }
+}

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

@@ -230,6 +230,11 @@ namespace obelisk {
             joint_state.joint_vel.resize(num_motors_);
             joint_state.joint_names.resize(num_motors_);
 
+            if (this->pub_temps_) {
+                joint_state.motor_surface_temps.resize(num_motors_);
+                joint_state.motor_winding_temps.resize(num_motors_);
+            }
+
             size_t ind = 0;
             for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; ++i) {
                 {
@@ -246,14 +251,20 @@ namespace obelisk {
                 joint_state.joint_names.at(ind) = G1_29DOF_JOINT_NAMES[i];
                 joint_state.joint_pos.at(ind) = low_state.motor_state()[i].q();
                 joint_state.joint_vel.at(ind) = low_state.motor_state()[i].dq();
+
+                if (this->pub_temps_) {
+                    joint_state.motor_surface_temps.at(ind) = low_state.motor_state()[i].temperature()[0];
+                    joint_state.motor_winding_temps.at(ind) = low_state.motor_state()[i].temperature()[1];
+
+                }
 
                 ind++;
             }
 
             this->GetPublisher<obelisk_sensor_msgs::msg::ObkJointEncoders>(pub_joint_state_key_)->publish(joint_state);
 
             // IMU
-            obelisk_sensor_msgs::msg::ObkImu imu_state;
+            sensor_msgs::msg::Imu imu_state;
             imu_state.header.stamp = this->now();
             imu_state.angular_velocity.x = low_state.imu_state().gyroscope()[0];
             imu_state.angular_velocity.y = low_state.imu_state().gyroscope()[1];
@@ -268,7 +279,7 @@ namespace obelisk {
             imu_state.linear_acceleration.y= low_state.imu_state().accelerometer()[1];
             imu_state.linear_acceleration.z = low_state.imu_state().accelerometer()[2];
 
-            this->GetPublisher<obelisk_sensor_msgs::msg::ObkImu>(pub_imu_state_key_)->publish(imu_state);
+            this->GetPublisher<sensor_msgs::msg::Imu>(pub_imu_state_key_)->publish(imu_state);
 
             // Log motor data if logging is enabled
             if (logging_) {

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

@@ -195,7 +195,7 @@ namespace obelisk {
             this->GetPublisher<obelisk_sensor_msgs::msg::ObkJointEncoders>(pub_joint_state_key_)->publish(joint_state);
 
             // IMU
-            obelisk_sensor_msgs::msg::ObkImu imu_state;
+            sensor_msgs::msg::Imu imu_state;
             imu_state.header.stamp = this->now();
             imu_state.angular_velocity.x = low_state.imu_state().gyroscope()[0];
             imu_state.angular_velocity.y = low_state.imu_state().gyroscope()[1];
@@ -210,7 +210,7 @@ namespace obelisk {
             imu_state.linear_acceleration.y= low_state.imu_state().accelerometer()[1];
             imu_state.linear_acceleration.z = low_state.imu_state().accelerometer()[2];
 
-            this->GetPublisher<obelisk_sensor_msgs::msg::ObkImu>(pub_imu_state_key_)->publish(imu_state);
+            this->GetPublisher<sensor_msgs::msg::Imu>(pub_imu_state_key_)->publish(imu_state);
 
             if (logging_) {
                 std::lock_guard<std::mutex> lk(motor_state_mtx_);

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

@@ -7,6 +7,7 @@
 #include "obelisk_control_msgs/msg/pd_feed_forward.h"
 #include "obelisk_control_msgs/msg/execution_fsm.hpp"
 #include "obelisk_control_msgs/msg/velocity_command.hpp"
+#include <sensor_msgs/msg/imu.hpp>
 #include <filesystem>
 #include <chrono>
 #include <iomanip>
@@ -54,7 +55,7 @@ namespace obelisk {
 
             // Additional Publishers
             this->RegisterObkPublisher<obelisk_sensor_msgs::msg::ObkJointEncoders>("pub_sensor_setting", pub_joint_state_key_);
-            this->RegisterObkPublisher<obelisk_sensor_msgs::msg::ObkImu>("pub_imu_setting", pub_imu_state_key_);
+            this->RegisterObkPublisher<sensor_msgs::msg::Imu>("pub_imu_setting", pub_imu_state_key_);
 
             // Register Execution FSM Subscriber
             this->RegisterObkSubscription<unitree_fsm_msg>(
@@ -88,22 +89,26 @@ namespace obelisk {
                 auto time_t = std::chrono::system_clock::to_time_t(now);
                 std::stringstream ss;
                 ss << std::put_time(std::localtime(&time_t), "%Y-%m-%d_%H-%M-%S");
-                
+
                 std::filesystem::path unitree_logs_dir = "unitree_" + robot_name + "_logs";
                 std::filesystem::path session_dir = unitree_logs_dir / ss.str();
-                
+
                 std::filesystem::create_directories(session_dir);
                 log_dir_path_ = session_dir.string();
                 RCLCPP_INFO_STREAM(this->get_logger(), "Created logging directory: " << session_dir);
 
                 log_count_ = 0;
 
                 startup_time_ = this->now();
-                InitializeLogging();
-                // TODO: Tie timer callback to WriteMotorData()
+                // NOTE: InitializeLogging and timer registration are deferred to on_activate
+                // because they call pure virtual methods that aren't available during construction.
                 this->RegisterObkTimer("timer_logging_setting", timer_logging_key_,
                                    std::bind(&ObeliskUnitreeInterface<MotorStateType, N>::WriteMotorData, this));
             }
+
+            // Optional motor temperature publisher
+            this->declare_parameter<bool>("pub_motor_temp", false);
+            pub_temps_ = this->get_parameter("pub_motor_temp").as_bool();
         }
 
         rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_activate(
@@ -117,6 +122,10 @@ namespace obelisk {
                 ReleaseUnitreeMotionControl();
             }
 
+            if (logging_) {
+                InitializeLogging();
+            }
+
             return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
         }
 
@@ -301,6 +310,9 @@ namespace obelisk {
         std::array<MotorStateType, N> motor_state_;
         mutable std::mutex motor_state_mtx_;
 
+        // Temp publishing
+        bool pub_temps_;
+
         // ---------- Gains ---------- //
         std::vector<double> kp_;
         std::vector<double> kd_;

obelisk/cpp/obelisk_cpp/CMakeLists.txt

@@ -34,6 +34,9 @@ option(USE_MUJOCO "Enable Mujoco support" ${USE_MUJOCO_DEFAULT})
 message(STATUS "USE_MUJOCO option set to: ${USE_MUJOCO}")
 
 if(USE_MUJOCO)
+  find_package(Eigen3 REQUIRED)
+  find_package(yaml-cpp REQUIRED)
+
   include(FetchContent)
   set(MUJOCO_VERSION "3.1.6" CACHE STRING "mujoco version")
   set(COMP_ARCH "x86_64" CACHE STRING "computer architecture")
@@ -61,7 +64,7 @@ endif()
 # ------- Source files ------- #
 message(STATUS "cmake source dir: ${CMAKE_CURRENT_SOURCE_DIR}")
 set(CORE_INC "${CMAKE_CURRENT_SOURCE_DIR}/include")
-
+set(LIDAR_INC "${CMAKE_CURRENT_SOURCE_DIR}/lidar")
 # These are not currently used, but might be used in the future
 # set(OBELISK_CPP_HEADER_LIST
 #         "${CORE_INC}/obelisk_node.h"
@@ -79,8 +82,8 @@ target_include_directories(ObkCore INTERFACE ${CORE_INC})
 
 # Conditionally add Mujoco dependencies
 if(USE_MUJOCO)
-  target_include_directories(ObkCore INTERFACE ${mujoco_SOURCE_DIR}/include)
-  target_link_libraries(ObkCore INTERFACE mujoco-lib glfw)
+  target_include_directories(ObkCore INTERFACE ${mujoco_SOURCE_DIR}/include ${LIDAR_INC})
+  target_link_libraries(ObkCore INTERFACE mujoco-lib glfw Eigen3::Eigen yaml-cpp)
 
   # Define compile definition for conditional compilation
   target_compile_definitions(ObkCore INTERFACE OBELISK_USE_MUJOCO)