General Improvements

obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h

@@ -328,46 +328,6 @@ namespace obelisk {
             mjr_freeContext(&con);
         }
 
-        obelisk_sensor_msgs::msg::TrueSimState PublishTrueSimState() override {
-            RCLCPP_WARN_STREAM_ONCE(this->get_logger(),
-                                    "The true sim state currently only works for floating base robots!");
-            static constexpr int DIM_FLOATING_BASE = 7;
-            static constexpr int DIM_FLOATING_VEL  = 6;
-
-            obelisk_sensor_msgs::msg::TrueSimState msg;
-
-            std::lock_guard<std::mutex> lock(sensor_data_mut_);
-
-            msg.header.frame_id = "world";
-            msg.header.stamp    = this->now();
-
-            // TODO: Base link name
-            // TODO: Joint names
-            // TODO: Handle both floating and fixed base
-
-            // Configuration
-            for (int i = 0; i < DIM_FLOATING_BASE; i++) {
-                msg.q_base.emplace_back(this->data_->qpos[i]);
-            }
-
-            for (int i = DIM_FLOATING_BASE; i < this->model_->nq; i++) {
-                msg.q_joints.emplace_back(this->data_->qpos[i]);
-            }
-
-            // Velocity
-            for (int i = 0; i < DIM_FLOATING_VEL; i++) {
-                msg.v_base.emplace_back(this->data_->qvel[i]);
-            }
-
-            for (int i = DIM_FLOATING_VEL; i < this->model_->nv; i++) {
-                msg.v_joints.emplace_back(this->data_->qvel[i]);
-            }
-
-            this->template GetPublisher<obelisk_sensor_msgs::msg::TrueSimState>(this->state_pub_key_)->publish(msg);
-
-            return msg;
-        }
-
       private:
         // ---------------------------------------------- //
         // ----------- Multithreading Support ----------- //

obelisk/cpp/obelisk_cpp/include/obelisk_sim_robot.h

@@ -20,14 +20,8 @@ namespace obelisk {
         using TrueSimState = obelisk_sensor_msgs::msg::TrueSimState;
 
       public:
-        explicit ObeliskSimRobot(const std::string& name, const std::string& state_timer_key = "timer_true_sim_state",
-                                 const std::string& state_pub_key = "pub_true_sim_state")
-            : ObeliskRobot<ControlMessageT>(name), state_timer_key_(state_timer_key), state_pub_key_(state_pub_key) {
-
-            // Register Components
-            this->RegisterObkTimer("timer_true_sim_state_setting", state_timer_key_,
-                                   std::bind(&ObeliskSimRobot::PublishTrueSimState, this));
-            this->template RegisterObkPublisher<TrueSimState>("pub_true_sim_state_setting", state_pub_key_);
+        explicit ObeliskSimRobot(const std::string& name)
+            : ObeliskRobot<ControlMessageT>(name) {
 
             stop_thread_ = false;
         }
@@ -122,18 +116,6 @@ namespace obelisk {
             }
         }
 
-        /**
-         * @brief Publish the TrueSimState of the simulator.
-         *
-         * This is the timer callback that publishes the TrueSimState and is expected to call
-         * self.publisher_true_sim_state internally. Note that the true sim state message is still returned afterwards,
-         * mostly for logging/debugging purposes. The publish call is the important part, NOT the returned value, since
-         * the topic is what the ObeliskEstimator subscribes to.
-         *
-         * @return obelisk_true_sim_state_msg: An Obelisk message type containing the true sim state.
-         */
-        virtual TrueSimState PublishTrueSimState() = 0;
-
         /**
          * @brief Run the simulator
          *
@@ -161,9 +143,6 @@ namespace obelisk {
         // Flag to signal stopping the thread
         std::atomic<bool> stop_thread_;
 
-        std::string state_timer_key_;
-        std::string state_pub_key_;
-
       private:
     };
 } // namespace obelisk

obelisk_ws/src/cpp/obelisk_viz_cpp/CMakeLists.txt

@@ -7,6 +7,7 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(fastcdr REQUIRED)  # Required for ament transitive dependencies
 
 # Get the Obelisk library
 include(FetchContent)

obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml

@@ -59,16 +59,6 @@ onboard:
         height_map_grid_spacing: 0.1
         height_map_geom_group: [0]
         # camera_name: "fixed_cam"  # You can select a camera to use here
-      publishers:
-        - ros_parameter: pub_true_sim_state_setting
-          topic: /obelisk/dummy/true_sim_state
-          history_depth: 10
-          callback_group: None
-      timers:
-        - ros_parameter: timer_true_sim_state_setting
-          history_depth: 10
-          timer_period_sec: 0.01
-          callback_group: None
       subscribers:
         - ros_parameter: sub_ctrl_setting
           topic: /obelisk/dummy/ctrl