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/place/series_legged_world.stl b/rm_gazebo/worlds/place/series_legged_world.stl new file mode 100644 index 00000000..eb80897f Binary files /dev/null and b/rm_gazebo/worlds/place/series_legged_world.stl differ 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 + + + diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index f7d54b14..51625a47 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -59,6 +60,7 @@ class RefereeBase virtual void trackCallBack(const rm_msgs::TrackDataConstPtr& data); virtual void deployDistanceCallBack(const geometry_msgs::PointConstPtr& 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); @@ -94,6 +96,7 @@ class RefereeBase ros::Subscriber track_sub_; ros::Subscriber deploy_distance_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_; @@ -125,6 +128,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{}, @@ -149,6 +153,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 a61a592c..5c1eb5be 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 @@ -172,4 +174,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 b66d9edf..14d1c025 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 @@ -449,4 +453,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 7de785a6..97f3987c 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -33,6 +33,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::deploy_distance_sub_ = nh.subscribe("/base2target", 10, &RefereeBase::deployDistanceCallBack, this); @@ -132,6 +136,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"); @@ -171,6 +182,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") @@ -255,6 +268,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_) @@ -402,6 +418,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(); } @@ -563,6 +581,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 3ebdb597..3620aae9 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -253,5 +253,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 f4b2d2b4..1df0f8df 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -308,6 +308,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++)