From 7e4b44d0fe2e6ca456b4affab57aaa2399909532 Mon Sep 17 00:00:00 2001 From: Zachary Date: Thu, 15 Aug 2024 00:50:28 +0000 Subject: [PATCH 1/9] added headers to messages --- .../obelisk_msgs/obelisk_control_msgs/msg/PDFeedForward.msg | 3 +++ .../obelisk_msgs/obelisk_control_msgs/msg/PositionSetpoint.msg | 3 +++ .../obelisk_msgs/obelisk_estimator_msgs/msg/EstimatedState.msg | 2 ++ 3 files changed, 8 insertions(+) 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 From 5a6c566869f19786dcf6ec7bd0f64db79a9c007d Mon Sep 17 00:00:00 2001 From: Zachary Date: Thu, 15 Aug 2024 15:40:03 +0000 Subject: [PATCH 2/9] changed all the sensor measurements to use ros time and in a header --- .../cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h | 10 +++++++--- .../obelisk_sensor_msgs/msg/ObkJointEncoders.msg | 4 +++- 2 files changed, 10 insertions(+), 4 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 index 36f183d5..f600494a 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -598,7 +598,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 +680,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 +792,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); }; 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..c8e33589 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,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 From c9580ed44f9046d6cd3b0d1178f62eec5049c301 Mon Sep 17 00:00:00 2001 From: Zachary Date: Thu, 15 Aug 2024 17:32:29 +0000 Subject: [PATCH 3/9] [wip] filled out the true sim state. currently only works for floating base --- .../include/obelisk_mujoco_sim_robot.h | 33 +++++++++++++++++++ .../obelisk_sensor_msgs/msg/TrueSimState.msg | 18 ++++++++++ .../src/obelisk_ros/config/dummy_cpp.yaml | 11 ++++++- 3 files changed, 61 insertions(+), 1 deletion(-) 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 f600494a..009a76fe 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,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 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(this->state_pub_key_)->publish(msg); + return msg; } 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..4e63b5f5 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,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 diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml index 68f4c196..28a016cf 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml @@ -65,7 +65,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 From 5a214735c748252bbaabf38bc9733d6b4f630ed7 Mon Sep 17 00:00:00 2001 From: Zachary Date: Thu, 15 Aug 2024 20:17:27 +0000 Subject: [PATCH 4/9] fixed callback group bug. added more logging for cbgs. addded a cbg in the dummy test --- obelisk/cpp/obelisk_cpp/include/obelisk_node.h | 15 ++++++++++++--- obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml | 5 +++-- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h b/obelisk/cpp/obelisk_cpp/include/obelisk_node.h index aa8d8fdb..3d270cc1 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 @@ -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_ros/config/dummy_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml index 28a016cf..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 From 0fe8cb04a89a63046e9948a566fa1e0281b1745d Mon Sep 17 00:00:00 2001 From: Zachary Date: Thu, 15 Aug 2024 20:18:54 +0000 Subject: [PATCH 5/9] fixed bug where I was looking for the wrong capitalization --- obelisk/cpp/obelisk_cpp/include/obelisk_node.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h b/obelisk/cpp/obelisk_cpp/include/obelisk_node.h index 3d270cc1..13072cd2 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_node.h @@ -789,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; From e8b482e41a50c77162032cc456540a51ca088641 Mon Sep 17 00:00:00 2001 From: alberthli Date: Thu, 15 Aug 2024 23:36:59 +0000 Subject: [PATCH 6/9] uncomment stamp --- .../obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c8e33589..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 @@ -4,7 +4,7 @@ string MESSAGE_NAME="ObkJointEncoders" std_msgs/Header header -#builtin_interfaces/Time stamp +builtin_interfaces/Time stamp float64[] joint_pos float64[] joint_vel From a2af4c7da0b43bee01fcf2a2a2dbf9c3b497b15a Mon Sep 17 00:00:00 2001 From: Zachary Date: Thu, 15 Aug 2024 23:56:17 +0000 Subject: [PATCH 7/9] clean up and test fixes --- .../include/obelisk_mujoco_sim_robot.h | 18 ++++++++---------- .../obelisk_sensor_msgs/msg/TrueSimState.msg | 5 ----- tests/tests_cpp/test_controller.cpp | 2 +- tests/tests_cpp/test_estimator.cpp | 2 +- tests/tests_cpp/test_node.cpp | 2 +- tests/tests_cpp/test_robot.cpp | 2 +- tests/tests_cpp/test_sensor.cpp | 2 +- tests/tests_cpp/test_sim_robot.cpp | 2 +- 8 files changed, 14 insertions(+), 21 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 index 009a76fe..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,8 +277,10 @@ namespace obelisk { protected: obelisk_sensor_msgs::msg::TrueSimState PublishTrueSimState() override { - static constexpr int FLOATING_BASE = 7; - static constexpr int FLOATING_VEL = 6; + 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; @@ -292,20 +294,20 @@ namespace obelisk { // TODO: Handle both floating and fixed base // Configuration - for (int i = 0; i < FLOATING_BASE; i++) { + for (int i = 0; i < DIM_FLOATING_BASE; i++) { msg.q_base.emplace_back(this->data_->qpos[i]); } - for (int i = FLOATING_BASE; i < this->model_->nq; 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 < FLOATING_VEL; i++) { + for (int i = 0; i < DIM_FLOATING_VEL; i++) { msg.v_base.emplace_back(this->data_->qvel[i]); } - for (int i = FLOATING_VEL; i < this->model_->nv; i++) { + for (int i = DIM_FLOATING_VEL; i < this->model_->nv; i++) { msg.v_joints.emplace_back(this->data_->qvel[i]); } @@ -631,7 +633,6 @@ namespace obelisk { << mj_sensor_types.at(i)); } - // GetTimeFromSim(msg.stamp.sec, msg.stamp.nanosec); msg.header.stamp = this->now(); } publisher->publish(msg); @@ -714,7 +715,6 @@ namespace obelisk { } msg.header.stamp = this->now(); - // GetTimeFromSim(msg.header.stamp.sec, msg.header.stamp.nanosec); } publisher->publish(msg); }; @@ -825,9 +825,7 @@ namespace obelisk { << mj_sensor_types.at(i)); } - // 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); }; 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 4e63b5f5..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 @@ -13,8 +13,3 @@ string[] joint_names # Velocities float64[] v_base float64[] v_joints - -# Contact forces -# Vector3[] contact_forces - -# Applied control 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_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..433b3ab4 100644 --- a/tests/tests_cpp/test_robot.cpp +++ b/tests/tests_cpp/test_robot.cpp @@ -6,7 +6,7 @@ namespace obelisk { class ObeliskRobotTester : public ObeliskRobot { public: ObeliskRobotTester() : ObeliskRobot("obelisk_sensor_tester") { - this->set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic4")); + this->set_parameter(rclcpp::Parameter("sub_ctrl_settings", "topic:topic4")); this->set_parameter(rclcpp::Parameter("callback_group_setting", "")); } 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")); } From ec8a7d671a502db9047d2003bc9964c42126dd49 Mon Sep 17 00:00:00 2001 From: Zachary Date: Fri, 16 Aug 2024 00:03:54 +0000 Subject: [PATCH 8/9] caught a few more typos --- tests/tests_cpp/test_mujoco_sim_robot.cpp | 2 +- tests/tests_cpp/test_robot.cpp | 2 +- tests/tests_cpp/test_viz_robot.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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_robot.cpp b/tests/tests_cpp/test_robot.cpp index 433b3ab4..379128f1 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_settings", "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_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() { From 2ae9068a1d2a07fa8795f0b7fe24e1dddb23d473 Mon Sep 17 00:00:00 2001 From: Zach Olkin Date: Fri, 16 Aug 2024 04:58:41 +0000 Subject: [PATCH 9/9] fixed failing tests --- tests/tests_cpp/claude_obelisk_node_test.cpp | 2 +- tests/tests_cpp/test_robot.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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_robot.cpp b/tests/tests_cpp/test_robot.cpp index 379128f1..0b0f8faa 100644 --- a/tests/tests_cpp/test_robot.cpp +++ b/tests/tests_cpp/test_robot.cpp @@ -6,7 +6,7 @@ namespace obelisk { class ObeliskRobotTester : public ObeliskRobot { public: ObeliskRobotTester() : ObeliskRobot("obelisk_sensor_tester") { - this->set_parameter(rclcpp::Parameter("sub_ctrl_settings", "topic:topic4")); + this->set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic4")); this->set_parameter(rclcpp::Parameter("callback_group_settings", "")); }