diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index 36f183d5..470560ab 100644 --- 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 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(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 index aa8d8fdb..13072cd2 100644 --- 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("callback_group_setting", ""); + this->declare_parameter("callback_group_settings", ""); // ROS parameter designed to let the user feed a file path for their own code this->declare_parameter("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("callback_group_setting", ""); + this->declare_parameter("callback_group_settings", ""); // ROS parameter designed to let the user feed a file path for their own code this->declare_parameter("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& 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 index f46f2006..725d4aed 100644 --- 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 index 051c593d..6b17e0b2 100644 --- 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 index ee50c264..a9c9f1cd 100644 --- 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 index d4f8cf72..d957f04c 100644 --- 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 index 1256eca9..f05caad1 100644 --- 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 index 68f4c196..c242f890 100644 --- 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 index 9f7816da..48ce666e 100644 --- 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 config_map{{"non_obelisk", "true"}}; + std::map 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 index 49d08e1e..c9389e14 100644 --- 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 index df190de0..0ee26401 100644 --- 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 index c2e61e05..d36a3037 100644 --- 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 index b7836add..01aadf29 100644 --- 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 index c9f0a14a..0b0f8faa 100644 --- 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 index 212d756c..e8cc891d 100644 --- 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 index 78909e4f..3d3eee62 100644 --- 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 index cfe3e4cf..fc182f67 100644 --- 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() {