Merge pull request #114 from Caltech-AMBER/message-stamps

Message stamps and Control Group Fixes
Caltech-AMBER · Aug 16, 2024 · b79117a · b79117a
2 parents 6af91e2 + 2ae9068
commit b79117a
Show file tree

Hide file tree

Showing 17 changed files with 96 additions and 19 deletions.
diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h
@@ -277,7 +277,42 @@ namespace obelisk {
 
       protected:
         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;
         }
 
@@ -598,7 +633,7 @@ namespace obelisk {
                                           << mj_sensor_types.at(i));
                         }
 
-                        GetTimeFromSim(msg.stamp.sec, msg.stamp.nanosec);
+                        msg.header.stamp = this->now();
                     }
                     publisher->publish(msg);
                 };
@@ -679,7 +714,7 @@ namespace obelisk {
                                           << mj_sensor_types.at(i));
                         }
 
-                        GetTimeFromSim(msg.header.stamp.sec, msg.header.stamp.nanosec);
+                        msg.header.stamp = this->now();
                     }
                     publisher->publish(msg);
                 };
@@ -790,7 +825,7 @@ namespace obelisk {
                                           << mj_sensor_types.at(i));
                         }
 
-                        GetTimeFromSim(msg.header.stamp.sec, msg.header.stamp.nanosec);
+                        msg.header.stamp = this->now();
                     }
                     publisher->publish(msg);
                 };

diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h b/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);
                 }
             }
 

diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_control_msgs/msg/PDFeedForward.msg b/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

diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_control_msgs/msg/PositionSetpoint.msg b/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
diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_estimator_msgs/msg/EstimatedState.msg b/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

diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg
@@ -2,6 +2,8 @@
 
 string MESSAGE_NAME="ObkJointEncoders"
 
+std_msgs/Header header
+
 builtin_interfaces/Time stamp
 
 float64[] joint_pos

diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/TrueSimState.msg b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/TrueSimState.msg
@@ -1,2 +1,15 @@
 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
diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml b/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

diff --git a/tests/tests_cpp/claude_obelisk_node_test.cpp b/tests/tests_cpp/claude_obelisk_node_test.cpp
@@ -68,7 +68,7 @@ TEST_CASE("GetIsObeliskMsg", "[ObeliskNode]") {
     rclcpp::init(0, nullptr);
 
     TestObeliskNode node;
-    std::map<std::string, std::string> config_map{{"non_obelisk", "true"}};
+    std::map<std::string, std::string> config_map{{"non_obelisk", "True"}};
 
     CHECK(node.GetIsObeliskMsg(config_map) == false);
 

diff --git a/tests/tests_cpp/test_controller.cpp b/tests/tests_cpp/test_controller.cpp
@@ -9,7 +9,7 @@ namespace obelisk {
             this->set_parameter(rclcpp::Parameter("timer_ctrl_setting", "timer_period_sec:1"));
             this->set_parameter(rclcpp::Parameter("pub_ctrl_setting", "topic:topic1"));
             this->set_parameter(rclcpp::Parameter("sub_est_setting", "topic:topic2"));
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", ""));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", ""));
         }
 
         void Configure() {

diff --git a/tests/tests_cpp/test_estimator.cpp b/tests/tests_cpp/test_estimator.cpp
@@ -8,7 +8,7 @@ namespace obelisk {
         ObeliskEstimatorTester() : ObeliskEstimator("obelisk_estimator_tester") {
             this->set_parameter(rclcpp::Parameter("timer_est_setting", "timer_period_sec:1"));
             this->set_parameter(rclcpp::Parameter("pub_est_setting", "topic:topic1"));
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", ""));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", ""));
         }
 
         void Configure() {

diff --git a/tests/tests_cpp/test_mujoco_sim_robot.cpp b/tests/tests_cpp/test_mujoco_sim_robot.cpp
@@ -8,7 +8,7 @@ namespace obelisk {
         ObeliskMujocoTester() : ObeliskMujocoRobot("obelisk_mujoco_tester") {
             this->set_parameter(rclcpp::Parameter("timer_true_sim_state_setting", "topic:topic1,timer_period_sec:1"));
             this->set_parameter(rclcpp::Parameter("pub_true_sim_state_setting", "topic:topic4"));
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", "topic:topic2"));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", "topic:topic2"));
             this->set_parameter(rclcpp::Parameter(
                 "mujoco_setting", "model_xml_path:dummy/"
                                   "dummy.xml,n_u:1,sensor_settings:[{group_name=group1|dt=0.01|topic=topic1|sensor_"

diff --git a/tests/tests_cpp/test_node.cpp b/tests/tests_cpp/test_node.cpp
@@ -12,7 +12,7 @@ namespace obelisk {
     class ObeliskNodeTester : public ObeliskNode {
       public:
         ObeliskNodeTester() : ObeliskNode("obelisk_tester") {
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", "my_cbg:None"));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", "my_cbg:None"));
 
             on_configure(this->get_current_state());
         }

diff --git a/tests/tests_cpp/test_robot.cpp b/tests/tests_cpp/test_robot.cpp
@@ -7,7 +7,7 @@ namespace obelisk {
       public:
         ObeliskRobotTester() : ObeliskRobot("obelisk_sensor_tester") {
             this->set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic4"));
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", ""));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", ""));
         }
 
         void Configure() {

diff --git a/tests/tests_cpp/test_sensor.cpp b/tests/tests_cpp/test_sensor.cpp
@@ -6,7 +6,7 @@ namespace obelisk {
     class ObeliskSensorTester : public ObeliskSensor {
       public:
         ObeliskSensorTester() : ObeliskSensor("obelisk_sensor_tester") {
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", ""));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", ""));
         }
 
         void Configure() {

diff --git a/tests/tests_cpp/test_sim_robot.cpp b/tests/tests_cpp/test_sim_robot.cpp
@@ -8,7 +8,7 @@ namespace obelisk {
         ObeliskSimRobotTester() : ObeliskSimRobot("obelisk_sim_robot_tester") {
             this->set_parameter(rclcpp::Parameter("timer_true_sim_state_setting", "topic:topic3,timer_period_sec:1"));
             this->set_parameter(rclcpp::Parameter("pub_true_sim_state_setting", "topic:topic4"));
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", "topic:topic2"));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", "topic:topic2"));
             this->set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic5"));
         }
 

diff --git a/tests/tests_cpp/test_viz_robot.cpp b/tests/tests_cpp/test_viz_robot.cpp
@@ -17,7 +17,7 @@ namespace obelisk::viz {
             this->set_parameter(rclcpp::Parameter("pub_viz_joint_setting", "topic:topic1"));
             this->set_parameter(rclcpp::Parameter("sub_viz_est_setting", "topic:topic2"));
             this->set_parameter(rclcpp::Parameter("timer_viz_joint_setting", "timer_period_sec:1"));
-            this->set_parameter(rclcpp::Parameter("callback_group_setting", ""));
+            this->set_parameter(rclcpp::Parameter("callback_group_settings", ""));
         }
 
         void Configure() {