From 276b503bcb27d6110550034cee34a8de11a51e35 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Wed, 3 Jun 2026 20:20:11 +0800 Subject: [PATCH 1/3] Add CapacityRunOutFlashUi for managing low battery state in UI --- rm_referee/include/rm_referee/referee_base.h | 5 + rm_referee/include/rm_referee/ui/flash_ui.h | 55 +++++++ .../include/rm_referee/ui/time_change_ui.h | 139 +++++++++++++++++ rm_referee/src/referee.cpp | 2 +- rm_referee/src/referee_base.cpp | 27 ++++ rm_referee/src/ui/flash_ui.cpp | 34 +++++ rm_referee/src/ui/time_change_ui.cpp | 143 ++++++++++++++++++ 7 files changed, 404 insertions(+), 1 deletion(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 6217da4c..81f59386 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -7,6 +7,7 @@ #include #include #include +#include #include "rm_referee/ui/ui_base.h" #include "rm_referee/ui/trigger_change_ui.h" @@ -55,6 +56,7 @@ class RefereeBase virtual void cameraNameCallBack(const std_msgs::StringConstPtr& data); virtual void trackCallBack(const rm_msgs::TrackDataConstPtr& data); virtual void balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data); + virtual void leggedChassisStatusCallback(const rm_msgs::LeggedChassisStatusConstPtr& data); virtual void radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data); virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data); virtual void sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data); @@ -87,6 +89,7 @@ class RefereeBase ros::Subscriber camera_name_sub_; ros::Subscriber track_sub_; ros::Subscriber balance_state_sub_; + ros::Subscriber legged_chassis_status_sub_; ros::Subscriber radar_receive_sub_; ros::Subscriber map_sentry_sub_; ros::Subscriber sentry_to_referee_sub_; @@ -115,6 +118,7 @@ class RefereeBase RotationTimeChangeUi* rotation_time_change_ui_{}; LaneLineTimeChangeGroupUi* lane_line_time_change_ui_{}; BalancePitchTimeChangeGroupUi* balance_pitch_time_change_group_ui_{}; + std::vector leg_theta_time_change_group_uis_{}; PitchAngleTimeChangeUi* pitch_angle_time_change_ui_{}; ImageTransmissionAngleTimeChangeUi* image_transmission_angle_time_change_ui_{}; JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{}, @@ -137,6 +141,7 @@ class RefereeBase ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{}; CustomizeDisplayFlashUi* customize_display_flash_ui_{}; BurstFlashUi* burst_flash_ui_{}; + CapacityRunOutFlashUi* capacity_run_out_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; CustomInfoSender* custom_info_sender{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 4c9fe4c5..7dfa180c 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -6,6 +6,8 @@ #include "rm_referee/ui/ui_base.h" +#include + namespace rm_referee { class FlashUi : public UiBase @@ -170,4 +172,57 @@ class BurstFlashUi : public FlashUi void display(const ros::Time& time) override; ros::Time start_burst_time_; }; + +class CapacityRunOutFlashUi : public FlashUi +{ +public: + explicit CapacityRunOutFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : FlashUi(rpc_value, base, "capacity_run_out", graph_queue, character_queue) + { + // Defaults assume capacity_remain_charge is normalized to [0, 1]. + threshold_low_ = 0.2; + threshold_high_ = 0.4; + if (rpc_value.hasMember("data")) + { + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + try + { + if (data.hasMember("threshold_low")) + threshold_low_ = static_cast(data["threshold_low"]); + if (data.hasMember("threshold_high")) + threshold_high_ = static_cast(data["threshold_high"]); + if (data.hasMember("threshold")) + { + threshold_low_ = static_cast(data["threshold"]); + threshold_high_ = threshold_low_; + } + } + catch (XmlRpc::XmlRpcException&) + { + // Keep defaults. + } + } + if (threshold_high_ < threshold_low_) + threshold_high_ = threshold_low_; + + // Warning text should be configured via YAML (config.content). + if (!rpc_value.hasMember("config") || !rpc_value["config"].hasMember("start_position")) + { + // Screen center-ish and slightly upper. + graph_->setStartX(760); + graph_->setStartY(320); + } + } + + void updateCapacityData(const rm_msgs::PowerManagementSampleAndStatusData& data, const ros::Time& last_get_data_time); + +private: + void display(const ros::Time& time) override; + + double threshold_low_{ 0.20 }; + double threshold_high_{ 0.30 }; + bool low_state_{ false }; + bool has_state_{ false }; +}; } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/time_change_ui.h b/rm_referee/include/rm_referee/ui/time_change_ui.h index bc512091..6edcb1e2 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -6,6 +6,10 @@ #include "rm_referee/ui/ui_base.h" +#include + +#include + namespace rm_referee { class TimeChangeUi : public UiBase @@ -406,4 +410,139 @@ class TargetHpTimeChangeUi : public TimeChangeUi int target_hp_{}, target_id_{}; }; +class LegThetaTimeChangeGroupUi : public TimeChangeGroupUi +{ +public: + explicit LegThetaTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "leg_theta", graph_queue, character_queue) + { + XmlRpc::XmlRpcValue line_config; + line_config["type"] = "line"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("color")) + line_config["color"] = rpc_value["config"]["color"]; + else + line_config["color"] = "cyan"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("width")) + line_config["width"] = rpc_value["config"]["width"]; + else + line_config["width"] = 2; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("delay")) + line_config["delay"] = rpc_value["config"]["delay"]; + else + line_config["delay"] = 0.2; + + // Virtual rod is rendered as multiple short segments (dash effect). + XmlRpc::XmlRpcValue virtual_rod_config = line_config; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_color")) + virtual_rod_config["color"] = rpc_value["config"]["virtual_rod_color"]; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_width")) + virtual_rod_config["width"] = rpc_value["config"]["virtual_rod_width"]; + else + virtual_rod_config["width"] = 1; + + XmlRpc::XmlRpcValue rect_config; + rect_config["type"] = "rectangle"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("chassis_color")) + rect_config["color"] = rpc_value["config"]["chassis_color"]; + else + rect_config["color"] = line_config["color"]; + rect_config["width"] = line_config["width"]; + rect_config["delay"] = line_config["delay"]; + + if (rpc_value.hasMember("data")) + { + XmlRpc::XmlRpcValue data = rpc_value["data"]; + if (data.hasMember("draw_chassis")) + draw_chassis_ = static_cast(data["draw_chassis"]); + if (data.hasMember("chassis_start") && data.hasMember("chassis_end")) + { + chassis_start_[0] = static_cast(data["chassis_start"][0]); + chassis_start_[1] = static_cast(data["chassis_start"][1]); + chassis_end_[0] = static_cast(data["chassis_end"][0]); + chassis_end_[1] = static_cast(data["chassis_end"][1]); + } + else + { + ROS_WARN("LegThetaTimeChangeGroupUi: 'data.chassis_start/end' not defined, using default rectangle."); + chassis_start_[0] = 800; + chassis_start_[1] = 600; + chassis_end_[0] = 1120; + chassis_end_[1] = 720; + } + + if (data.hasMember("origin_point")) + { + origin_point_[0] = static_cast(data["origin_point"][0]); + origin_point_[1] = static_cast(data["origin_point"][1]); + } + else + { + // Default: rectangle center. + origin_point_[0] = (chassis_start_[0] + chassis_end_[0]) / 2; + origin_point_[1] = (chassis_start_[1] + chassis_end_[1]) / 2; + } + + if (data.hasMember("link1_length")) + link1_length_m_ = static_cast(data["link1_length"]); + if (data.hasMember("link2_length")) + link2_length_m_ = static_cast(data["link2_length"]); + if (data.hasMember("pixels_per_meter")) + pixels_per_meter_ = static_cast(data["pixels_per_meter"]); + if (data.hasMember("leg_side")) + leg_side_ = static_cast(data["leg_side"]); + } + else + { + ROS_WARN("LegThetaTimeChangeGroupUi config 's member 'data' not defined."); + } + + // Chassis rectangle (left=rear, right=front). + if (draw_chassis_) + { + rect_config["start_position"][0] = chassis_start_[0]; + rect_config["start_position"][1] = chassis_start_[1]; + rect_config["end_position"][0] = chassis_end_[0]; + rect_config["end_position"][1] = chassis_end_[1]; + graph_vector_.insert(std::make_pair("chassis", new Graph(rect_config, base_, id_++))); + } + + // Link 1 and Link 2. + line_config["start_position"][0] = origin_point_[0]; + line_config["start_position"][1] = origin_point_[1]; + line_config["end_position"][0] = origin_point_[0]; + line_config["end_position"][1] = origin_point_[1]; + graph_vector_.insert(std::make_pair("link1", new Graph(line_config, base_, id_++))); + graph_vector_.insert(std::make_pair("link2", new Graph(line_config, base_, id_++))); + } + + void calculatePointPosition(const rm_msgs::LeggedChassisStatusConstPtr& data, const ros::Time& time); + +private: + void updateConfig() override; + + int chassis_start_[2]{ 800, 600 }; + int chassis_end_[2]{ 1120, 720 }; + int origin_point_[2]{ 960, 660 }; // hip/pivot point in screen coordinates + + bool draw_chassis_{ true }; + + // IK config + double link1_length_m_{ 0.21 }; + double link2_length_m_{ 0.248 }; + double pixels_per_meter_{ 600.0 }; + std::string leg_side_{ "left" }; // "left" or "right" + + // Latest input (virtual rod) + double virtual_rod_length_m_{ 0.0 }; + double virtual_rod_theta_rad_{ 0.0 }; + + // Solved points (screen coordinates) + int knee_point_[2]{ 960, 660 }; + int foot_point_[2]{ 960, 660 }; + + // Keep IK solution continuous across updates (avoid knee flipping between the two possible IK branches). + bool knee_initialized_{ false }; +}; + } // namespace rm_referee diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 2d98e329..180afa9a 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -50,7 +50,7 @@ void Referee::read() return; uint8_t temp_buffer[256] = { 0 }; int frame_len; - if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) + if (ros::Time::now() - last_get_data_time_ > ros::Duration(5.0)) base_.referee_data_is_online_ = false; if (rx_len_ < k_unpack_buffer_length_) { diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index ccbc3a64..85e66ebb 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -31,6 +31,10 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/manual_to_referee", 10, &RefereeBase::manualDataCallBack, this); RefereeBase::camera_name_sub_ = nh.subscribe("/camera_name", 10, &RefereeBase::cameraNameCallBack, this); RefereeBase::balance_state_sub_ = nh.subscribe("/state", 10, &RefereeBase::balanceStateCallback, this); + const std::string legged_chassis_status_topic = + getParam(nh, "legged_chassis_status_topic", std::string("/controllers/chassis_controller/legged_chassis_status")); + RefereeBase::legged_chassis_status_sub_ = nh.subscribe( + legged_chassis_status_topic, 10, &RefereeBase::leggedChassisStatusCallback, this); RefereeBase::track_sub_ = nh.subscribe("/track", 10, &RefereeBase::trackCallBack, this); RefereeBase::map_sentry_sub_ = nh.subscribe("/map_sentry_data", 10, &RefereeBase::mapSentryCallback, this); @@ -121,6 +125,13 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "balance_pitch") balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"].getType() == XmlRpc::XmlRpcValue::TypeString) + { + const std::string ui_name = static_cast(rpc_value[i]["name"]); + if (ui_name.find("leg_theta") != std::string::npos) + leg_theta_time_change_group_uis_.push_back( + new LegThetaTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_)); + } if (rpc_value[i]["name"] == "engineer_joint1") engineer_joint1_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint1"); @@ -155,6 +166,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "spin") spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "capacity_run_out") + capacity_run_out_flash_ui_ = new CapacityRunOutFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); // if (rpc_value[i]["name"] == "deploy") // deploy_flash_ui_ = new DeployFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "hero_hit") @@ -235,6 +248,9 @@ void RefereeBase::addUi() lane_line_time_change_ui_->addForQueue(); if (balance_pitch_time_change_group_ui_) balance_pitch_time_change_group_ui_->addForQueue(); + for (auto* ui : leg_theta_time_change_group_uis_) + if (ui) + ui->addForQueue(); if (pitch_angle_time_change_ui_) pitch_angle_time_change_ui_->addForQueue(); // if (image_transmission_angle_time_change_ui_) @@ -378,6 +394,8 @@ void RefereeBase::capacityDataCallBack(const rm_msgs::PowerManagementSampleAndSt { if (capacitor_time_change_ui_ && !is_adding_) capacitor_time_change_ui_->updateRemainCharge(data.capacity_remain_charge, last_get_data_time); + if (capacity_run_out_flash_ui_ && !is_adding_) + capacity_run_out_flash_ui_->updateCapacityData(data, last_get_data_time); if (chassis_trigger_change_ui_ && !is_adding_) chassis_trigger_change_ui_->updateCapacityResetStatus(); } @@ -522,6 +540,15 @@ void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data if (balance_pitch_time_change_group_ui_) balance_pitch_time_change_group_ui_->calculatePointPosition(data, ros::Time::now()); } + +void RefereeBase::leggedChassisStatusCallback(const rm_msgs::LeggedChassisStatusConstPtr& data) +{ + if (is_adding_) + return; + for (auto* ui : leg_theta_time_change_group_uis_) + if (ui) + ui->calculatePointPosition(data, ros::Time::now()); +} void RefereeBase::sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data) { if (sentry_to_radar_) diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index ca77b696..802ebc24 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -229,5 +229,39 @@ void BurstFlashUi::updateBurstTimeData(const rm_msgs::ManualToReferee::ConstPtr& start_burst_time_ = data->start_burst_time; display(ros::Time::now()); } + +void CapacityRunOutFlashUi::display(const ros::Time& time) +{ + FlashUi::updateFlashUiForQueue(time, low_state_, true); +} + +void CapacityRunOutFlashUi::updateCapacityData(const rm_msgs::PowerManagementSampleAndStatusData& data, + const ros::Time& last_get_data_time) +{ + const double capacity = data.capacity_remain_charge; + bool new_low_state = low_state_; + + // First message: only send UI when low; avoid spamming an initial DELETE. + if (!has_state_) + { + has_state_ = true; + low_state_ = capacity < threshold_low_; + if (low_state_) + display(last_get_data_time); + return; + } + + // With hysteresis to avoid flicker around the boundary. + if (!low_state_ && capacity < threshold_low_) + new_low_state = true; + else if (low_state_ && capacity > threshold_high_) + new_low_state = false; + + if (new_low_state != low_state_) + { + low_state_ = new_low_state; + display(last_get_data_time); + } +} } // namespace rm_referee // namespace rm_referee diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 6256855b..ea85c363 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -278,6 +278,149 @@ void BalancePitchTimeChangeGroupUi::calculatePointPosition(const rm_msgs::Balanc updateForQueue(); } +namespace +{ +inline int clampUiCoord(int v) +{ + // GraphConfig uses 11-bit unsigned fields. + if (v < 0) + return 0; + if (v > 2047) + return 2047; + return v; +} +} // namespace + +void LegThetaTimeChangeGroupUi::calculatePointPosition(const rm_msgs::LeggedChassisStatusConstPtr& data, + const ros::Time& time) +{ + (void)time; + if (!data) + return; + if (pixels_per_meter_ <= 0.0 || link1_length_m_ <= 0.0 || link2_length_m_ <= 0.0) + return; + + // Virtual rod inputs. + if (leg_side_ == "right") + { + virtual_rod_length_m_ = data->right_leg_length; + virtual_rod_theta_rad_ = data->right_leg_theta; + } + else + { + virtual_rod_length_m_ = data->left_leg_length; + virtual_rod_theta_rad_ = data->left_leg_theta; + } + + // Screen coordinate convention in this UI codebase: + // x increases to the right; y increases upward (origin at bottom-left of a 1920x1080 screen). + // Angle is defined with 0 pointing downward (-y) and clockwise positive. + const Eigen::Vector2d A(origin_point_[0], origin_point_[1]); + const double L_virtual_px = virtual_rod_length_m_ * pixels_per_meter_; + // With y-up coordinates, the downward direction is (0, -1). A clockwise rotation by +theta from downward yields + // direction = (-sin(theta), -cos(theta)). + const Eigen::Vector2d dir(-std::sin(virtual_rod_theta_rad_), -std::cos(virtual_rod_theta_rad_)); + Eigen::Vector2d E = A + dir * L_virtual_px; + + const double L1 = link1_length_m_ * pixels_per_meter_; + const double L2 = link2_length_m_ * pixels_per_meter_; + + Eigen::Vector2d v = E - A; + double d = v.norm(); + if (d < 1e-6) + { + // Avoid division by zero; place foot a tiny bit below the hip. + v = Eigen::Vector2d(0.0, 1e-3); + d = v.norm(); + E = A + v; + } + + // Clamp distance into reachable annulus to keep sqrt well-defined. + const double d_min = std::abs(L1 - L2) + 1e-3; + const double d_max = (L1 + L2) - 1e-3; + double d_clamped = d; + if (d_clamped < d_min) + d_clamped = d_min; + if (d_clamped > d_max) + d_clamped = d_max; + if (std::abs(d_clamped - d) > 1e-9) + { + E = A + v / d * d_clamped; + v = E - A; + d = d_clamped; + } + + const Eigen::Vector2d u = v / d; // unit direction from hip to foot + const double a = (L1 * L1 - L2 * L2 + d * d) / (2.0 * d); + double h2 = L1 * L1 - a * a; + if (h2 < 0.0) + h2 = 0.0; + const double h = std::sqrt(h2); + const Eigen::Vector2d P = A + a * u; + const Eigen::Vector2d perp(-u.y(), u.x()); + const Eigen::Vector2d K_pos = P + h * perp; + const Eigen::Vector2d K_neg = P - h * perp; + + // Two valid IK solutions exist (elbow-up / elbow-down). If we pick purely by geometry + // (e.g. "knee on the left"), the selected branch can flip abruptly when the target + // crosses a singularity, causing a visible discontinuity. Instead, keep continuity by + // choosing the candidate closest to the previous knee position. + Eigen::Vector2d K; + if (!knee_initialized_) + { + // Initial selection: prefer knee on the left (rear). In UI: left is smaller x. + K = (K_pos.x() <= K_neg.x()) ? K_pos : K_neg; + knee_initialized_ = true; + } + else + { + const Eigen::Vector2d K_prev(knee_point_[0], knee_point_[1]); + const double dist_pos = (K_pos - K_prev).squaredNorm(); + const double dist_neg = (K_neg - K_prev).squaredNorm(); + if (std::abs(dist_pos - dist_neg) < 1e-6) + K = (K_pos.x() <= K_neg.x()) ? K_pos : K_neg; + else + K = (dist_pos <= dist_neg) ? K_pos : K_neg; + } + + knee_point_[0] = clampUiCoord(static_cast(std::lround(K.x()))); + knee_point_[1] = clampUiCoord(static_cast(std::lround(K.y()))); + foot_point_[0] = clampUiCoord(static_cast(std::lround(E.x()))); + foot_point_[1] = clampUiCoord(static_cast(std::lround(E.y()))); + updateForQueue(); +} + +void LegThetaTimeChangeGroupUi::updateConfig() +{ + const Eigen::Vector2d A(origin_point_[0], origin_point_[1]); + const Eigen::Vector2d B(foot_point_[0], foot_point_[1]); + + for (auto& it : graph_vector_) + { + if (it.first == "chassis") + { + it.second->setStartX(clampUiCoord(chassis_start_[0])); + it.second->setStartY(clampUiCoord(chassis_start_[1])); + it.second->setEndX(clampUiCoord(chassis_end_[0])); + it.second->setEndY(clampUiCoord(chassis_end_[1])); + } + else if (it.first == "link1") + { + it.second->setStartX(clampUiCoord(origin_point_[0])); + it.second->setStartY(clampUiCoord(origin_point_[1])); + it.second->setEndX(clampUiCoord(knee_point_[0])); + it.second->setEndY(clampUiCoord(knee_point_[1])); + } + else if (it.first == "link2") + { + it.second->setStartX(clampUiCoord(knee_point_[0])); + it.second->setStartY(clampUiCoord(knee_point_[1])); + it.second->setEndX(clampUiCoord(foot_point_[0])); + it.second->setEndY(clampUiCoord(foot_point_[1])); + } + } +} + void PitchAngleTimeChangeUi::updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time) { for (unsigned int i = 0; i < data->name.size(); i++) From c654db6cd10f1842258531fca08d487b05cfd046 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 5 Jun 2026 20:49:22 +0800 Subject: [PATCH 2/3] Add series legged world and launch configuration for simulation --- rm_gazebo/launch/series_legged_world.launch | 32 +++++++++ rm_gazebo/worlds/series_legged.world | 73 +++++++++++++++++++++ 2 files changed, 105 insertions(+) create mode 100644 rm_gazebo/launch/series_legged_world.launch create mode 100644 rm_gazebo/worlds/series_legged.world diff --git a/rm_gazebo/launch/series_legged_world.launch b/rm_gazebo/launch/series_legged_world.launch new file mode 100644 index 00000000..11d7b156 --- /dev/null +++ b/rm_gazebo/launch/series_legged_world.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + r + + + + + + + diff --git a/rm_gazebo/worlds/series_legged.world b/rm_gazebo/worlds/series_legged.world new file mode 100644 index 00000000..58f75b91 --- /dev/null +++ b/rm_gazebo/worlds/series_legged.world @@ -0,0 +1,73 @@ + + + + + + + + + 0.001 + 1000 + + + + model://ground_plane + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 1 + 1 + + -2 -0.6 0.0 0 0 0 + + + + model://rm_gazebo/worlds/place/series_legged_world.stl + 1 1 1 + + + + + + + model://rm_gazebo/worlds/place/series_legged_world.stl + 1 1 1 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 1 + + + From 03d737c89ee859f05460882a758795f47da376dd Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 14 Jun 2026 11:02:41 +0800 Subject: [PATCH 3/3] Add series_legged_world. --- rm_gazebo/worlds/place/series_legged_world.stl | Bin 0 -> 8684 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 rm_gazebo/worlds/place/series_legged_world.stl diff --git a/rm_gazebo/worlds/place/series_legged_world.stl b/rm_gazebo/worlds/place/series_legged_world.stl new file mode 100644 index 0000000000000000000000000000000000000000..eb80897fbca5a6f3d6c6d7a14b5fb0fc42af9bf7 GIT binary patch literal 8684 zcmbW5YlxLq6o!u!)F@q$sD!BoQoBH-VM+v@bEbWSK-TaO#~=BAZl?Net@82Ec& zNga70Le9rLNT?<^&mf_Eqdc64c~Gk-4{UutGO#ze2`r?2@c^q42}iG+Rp)^^f)FMzAwj zLD(>lVDI2VGlb08)%Vywl(6*(9-^BzW5-9W4I{*SvoYn>d29B$C2M?CZPh5D-P3GL zJ%0A&E6zA$Bq!8LYZLR>G5N2vL8wLv?KhKo$b%^W zJCX>$s@ggY?4D##CdslDUyr;fp^z-_%;o-NN zjlH*>Ir7uC^?^{05`P@?LLhvE9paM*)|3dfg759C_Iqk{a|05A0j#hIHRHFp^8KN?0==Iih8y>7_B$|`fso7{9+4D9D)hMC0@i)WpGu1gm zy(J$?YlN0ijS|`miKsFUW|-5gtR=86HA+|?D1^^E_|VN`Z$ z@_SYCp|p07B~+t?_HiOYJ+x{$jkAgKXbG%KjS|)ede9Gt-o5(Eq3~SC{Q>#B+fS-O zUN+pb5k`>HiSnOaO*J{en=cVACqUDxQdzOlFWJS1YI1_K^`VhKzV=O7(KKv3SdNxv{>mn!2V~+K(1T+{ikHM7F`0cBni#1=g z(@l7BvpTHH})-jxV_MNLjgaqYCux~L7g0Yjj9P>d1 zXUzDp<6~(ZGJ6_w>_n^qLN%Cqc&H#KC&JOki5L9hy_atv&L<-IpeFi=cNcueT^LPK zrEaW%U>>AFaGpLQp&Ize2}DaGJ_lj7>&P?->hbtDDK4gWJvYsL|Z0!>tkJ>$9%8yJc`(LcNt+9 zAD!Oiy4@;hedw9sxjTC1+>_|To?7fWqQXG8FM5a13EWGRqke@v)!y3hG``v4Zug_m zbR%GqGI#!i|0xK{e%=rJW|=Z~s>unyccnbg?VgBZ;Y%mf_U-L$;nUF2xK6}TXNWk} z`Ckb<=P|7}b&4!eYK2{(!un86HzN6PK829S+NGl!$spgiu+QWae2}PU z>`Ktz?2n7*N6bSBwW7?ZSnrq{&IERsnNsFiK~!Y|=PoDs`M(KT>XXy@poW(G%{QE~ zk9dmW^PFb|6jv$})WAv!bI!Rh4pvIg3iG^O%RZ=qRZgUP7d{*;FnHrYRsymiVBqkd zX3o0=^USFn6*UDN-ybkv@&TiiA!K$2WbTOn>&N>qhpxW+>xa_!TJ&h%yZDY9dp9Sj zQ35jNaaNj*{v$u5>sJKvJ<6bGD8YN;Jl?v`I+lfl36=dFgmO!?QikYJZ>dz9YfsD2F8C8x- zy@Nm`ARrf+!^#im|ApG)tYJcjKB>`H9ImYz&f>Q|y8_|-r2nymTIrLz+1R{yYtQ1} z-S`+)d;{<+m~}ze=lO<5#!VnWjrxGh`LXkW6>d2A!{Cnis1~07+|I*4JR5x!nw$U~ z|CVw7B|Cb$5m=X2zV7L}XtUvOMxQl?YpVt`O7I)9?Gl^`)EPG!`U)~n6lxxCIPG>2 z)F{CYjpw{`Cx#miXASG}Erb3rkCoyLmlM=zgz7upiQ$IRepP?AcRROeBH#lZu7r)M zQ#pt2&9g-wIDS#fR6P*G(NI130UbK znyA8CAJ)QN(FeN(^RY|Rhpwd+)Sb3IKx3`^O;1;{glb6hn-^9xdll{~)&u(vn)T|`DY)u<2KSN{ScR3e)I literal 0 HcmV?d00001