Message stamps and Control Group Fixes

obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h

@@ -277,7 +277,40 @@ namespace obelisk {
 
       protected:
         obelisk_sensor_msgs::msg::TrueSimState PublishTrueSimState() override {
+            static constexpr int FLOATING_BASE = 7;
+            static constexpr int 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 < FLOATING_BASE; i++) {
+                msg.q_base.emplace_back(this->data_->qpos[i]);
+            }
+
+            for (int i = FLOATING_BASE; i < this->model_->nq; i++) {
+                msg.q_joints.emplace_back(this->data_->qpos[i]);
+            }
+
+            // Velocity
+            for (int i = 0; i < FLOATING_VEL; i++) {
+                msg.v_base.emplace_back(this->data_->qvel[i]);
+            }
+
+            for (int i = 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;
         }
 
@@ -598,7 +631,8 @@ namespace obelisk {
                                           << mj_sensor_types.at(i));
                         }
 
-                        GetTimeFromSim(msg.stamp.sec, msg.stamp.nanosec);
+                        // GetTimeFromSim(msg.stamp.sec, msg.stamp.nanosec);
+                        msg.header.stamp = this->now();
                     }
                     publisher->publish(msg);
                 };
@@ -679,7 +713,8 @@ namespace obelisk {
                                           << mj_sensor_types.at(i));
                         }
 
-                        GetTimeFromSim(msg.header.stamp.sec, msg.header.stamp.nanosec);
+                        msg.header.stamp = this->now();
+                        // GetTimeFromSim(msg.header.stamp.sec, msg.header.stamp.nanosec);
                     }
                     publisher->publish(msg);
                 };
@@ -790,7 +825,9 @@ namespace obelisk {
                                           << mj_sensor_types.at(i));
                         }
 
-                        GetTimeFromSim(msg.header.stamp.sec, msg.header.stamp.nanosec);
+                        // Note that this might cause weird issues with finite differencing - more testing is needed
+                        msg.header.stamp = this->now();
+                        // GetTimeFromSim(msg.header.stamp.sec, msg.header.stamp.nanosec);
                     }
                     publisher->publish(msg);
                 };

obelisk/cpp/obelisk_cpp/include/obelisk_node.h

@@ -203,7 +203,7 @@ namespace obelisk {
                              bool enable_communication_interface = true)
             : LifecycleNode(node_name, options, enable_communication_interface), CB_GROUP_NONE("None"),
               CB_GROUP_MUTUALLY_EXEC("MutuallyExclusiveCallbackGroup"), CB_GROUP_REENTRANT("ReentrantCallbackGroup") {
-            this->declare_parameter<std::string>("callback_group_setting", "");
+            this->declare_parameter<std::string>("callback_group_settings", "");
 
             // ROS parameter designed to let the user feed a file path for their own code
             this->declare_parameter<std::string>("params_path", "");
@@ -216,7 +216,7 @@ namespace obelisk {
                     bool enable_communication_interface = true)
             : LifecycleNode(node_name, namespace_, options, enable_communication_interface), CB_GROUP_NONE("None"),
               CB_GROUP_MUTUALLY_EXEC("MutuallyExclusiveCallbackGroup"), CB_GROUP_REENTRANT("ReentrantCallbackGroup") {
-            this->declare_parameter<std::string>("callback_group_setting", "");
+            this->declare_parameter<std::string>("callback_group_settings", "");
 
             // ROS parameter designed to let the user feed a file path for their own code
             this->declare_parameter<std::string>("params_path", "");
@@ -367,7 +367,7 @@ namespace obelisk {
             rclcpp_lifecycle::LifecycleNode::on_configure(prev_state);
 
             // Parse the configuration groups for this node
-            ParseCallbackGroupConfig(this->get_parameter("callback_group_setting").as_string());
+            ParseCallbackGroupConfig(this->get_parameter("callback_group_settings").as_string());
 
             CreatePublishers();
             CreateSubscriptions();
@@ -702,6 +702,10 @@ namespace obelisk {
                 // Get the callback group based on the string name
                 cbg = callback_groups_.at(GetCallbackGroupName(config_map));
             } catch (const std::exception& e) {
+                RCLCPP_ERROR_STREAM(this->get_logger(),
+                                    "Callback group "
+                                        << GetCallbackGroupName(config_map)
+                                        << " not found as a callback group. Initializing with the default group.");
             }
 
             // Create the timer
@@ -785,7 +789,7 @@ namespace obelisk {
          */
         bool GetIsObeliskMsg(const std::map<std::string, std::string>& config_map) {
             try {
-                if (config_map.at("non_obelisk") == "true") {
+                if (config_map.at("non_obelisk") == "True") {
                     return false;
                 } else {
                     return true;
@@ -864,12 +868,17 @@ namespace obelisk {
                 if (cbg.second == CB_GROUP_MUTUALLY_EXEC) {
                     callback_groups_.emplace(cbg.first,
                                              this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive));
+                    RCLCPP_INFO_STREAM(this->get_logger(),
+                                       "Creating a mutually exclusive callback group with name " << cbg.first);
                 } else if (cbg.second == CB_GROUP_REENTRANT) {
                     callback_groups_.emplace(cbg.first,
                                              this->create_callback_group(rclcpp::CallbackGroupType::Reentrant));
+                    RCLCPP_INFO_STREAM(this->get_logger(),
+                                       "Creating a reentrant callback group with name " << cbg.first);
                 } else {
                     // Node's default callback group
                     callback_groups_.emplace(cbg.first, nullptr);
+                    RCLCPP_WARN_STREAM(this->get_logger(), "Creating a default callback group with name " << cbg.first);
                 }
             }
 

obelisk_ws/src/obelisk_msgs/obelisk_control_msgs/msg/PDFeedForward.msg

@@ -1,4 +1,7 @@
 string MESSAGE_NAME="PDFeedForward"
+
+std_msgs/Header header
+
 float64[] u_mujoco
 float64[] pos_target
 float64[] vel_target

obelisk_ws/src/obelisk_msgs/obelisk_control_msgs/msg/PositionSetpoint.msg

@@ -1,3 +1,6 @@
 string MESSAGE_NAME="PositionSetpoint"
+
+std_msgs/Header header
+
 float64[] u_mujoco
 float64[] q_des

obelisk_ws/src/obelisk_msgs/obelisk_estimator_msgs/msg/EstimatedState.msg

@@ -1,5 +1,7 @@
 string MESSAGE_NAME="EstimatedState"
 
+std_msgs/Header header
+
 # Quantities required for viz
 float64[] q_base
 string base_link_name

obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg

@@ -2,7 +2,9 @@
 
 string MESSAGE_NAME="ObkJointEncoders"
 
-builtin_interfaces/Time stamp
+std_msgs/Header header
+
+#builtin_interfaces/Time stamp
 
 float64[] joint_pos
 float64[] joint_vel

obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/TrueSimState.msg

@@ -1,2 +1,20 @@
 string MESSAGE_NAME="TrueSimState"
 float64[] y
+
+std_msgs/Header header
+
+# Quantities required for viz
+float64[] q_base
+string base_link_name
+
+float64[] q_joints
+string[] joint_names
+
+# Velocities
+float64[] v_base
+float64[] v_joints
+
+# Contact forces
+# Vector3[] contact_forces
+
+# Applied control

obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml

@@ -4,7 +4,8 @@ onboard:
     - pkg: obelisk_control_cpp
       executable: example_position_setpoint_controller
       params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt
-      # callback_groups:
+      callback_groups:
+        test_cbg: MutuallyExclusiveCallbackGroup
       params:
         test_param: value_configured_in_yaml
       publishers:
@@ -28,7 +29,7 @@ onboard:
         - ros_parameter: timer_ctrl_setting
           # key: timer1
           timer_period_sec: 0.001
-          callback_group: None
+          callback_group: test_cbg
           # callback_key: timer_callback1
   estimation:
     - pkg: obelisk_estimation_cpp
@@ -65,7 +66,16 @@ onboard:
       params:
         ic_keyframe: ic
       # callback_groups:
-      # publishers:
+      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
           # key: sub1