heat_limit.h

Go to the documentation of this file.

/*******************************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2021, Qiayuan Liao
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *******************************************************************************/
 
//
// Created by qiayuan on 5/19/21.
//
 
#pragma once
 
#include <ros/ros.h>
#include <mutex>
#include <rm_msgs/GameRobotStatus.h>
#include <rm_msgs/PowerHeatData.h>
#include <rm_msgs/ShootCmd.h>
#include <rm_msgs/LocalHeatState.h>
#include <rm_msgs/LocalHeatData.h>
#include <std_msgs/Float64.h>
 
namespace rm_common
{
class HeatLimit
{
public:
  HeatLimit(ros::NodeHandle& nh)
 
  {
    if (!nh.getParam("low_shoot_frequency", low_shoot_frequency_))
      ROS_ERROR("Low shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("high_shoot_frequency", high_shoot_frequency_))
      ROS_ERROR("High shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("burst_shoot_frequency", burst_shoot_frequency_))
      ROS_ERROR("Burst shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("minimal_shoot_frequency", minimal_shoot_frequency_))
      ROS_ERROR("Minimal shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("outpost_shoot_frequency", outpost_shoot_frequency_))
      ROS_ERROR("Outpost shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("safe_shoot_frequency", safe_shoot_frequency_))
      ROS_ERROR("Safe shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("heat_coeff", heat_coeff_))
      ROS_ERROR("Heat coeff no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("type", type_))
      ROS_ERROR("Shooter type no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("local_heat_protect_threshold", heat_protect_threshold_))
      ROS_ERROR("Local heat protect threshold no defined (namespace: %s)", nh.getNamespace().c_str());
    if (!nh.getParam("buff_offset", buff_offset_))
      ROS_ERROR("Buff offset no defined (namespace: %s)", nh.getNamespace().c_str());
    nh.param("use_local_heat", use_local_heat_, true);
    if (type_ == "ID1_42MM")
      bullet_heat_ = 100.;
    else
      bullet_heat_ = 10.;
    local_heat_pub_ = nh.advertise<rm_msgs::LocalHeatData>("/local_heat_state/local_cooling_heat", 10);
    shoot_state_sub_ =
        nh.subscribe<rm_msgs::LocalHeatState>("/local_heat_state/shooter_state", 50, &HeatLimit::heatCB, this);
    timer_ = nh.createTimer(ros::Duration(0.1), std::bind(&HeatLimit::timerCB, this));
    fly_time_sub_ = nh.subscribe<std_msgs::Float64>("/controllers/gimbal_controller/bullet_solver/fly_time", 10,
                                                    &HeatLimit::flyTimeCB, this);
  }
 
  typedef enum
  {
    LOW = 0,
    HIGH = 1,
    BURST = 2,
    MINIMAL = 3
  } ShootHz;
 
  void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg)
  {
    std::lock_guard<std::mutex> lock(heat_mutex_);
    if (msg->has_shoot && last_shoot_state_ != msg->has_shoot)
    {
      local_shooter_cooling_heat_ += bullet_heat_;
      if (local_shooter_cooling_heat_ > 0)
        once_shoot_num_++;
    }
    last_shoot_state_ = msg->has_shoot;
  }
 
  void timerCB()
  {
    std::lock_guard<std::mutex> lock(heat_mutex_);
    if (local_shooter_cooling_heat_ > 0.0)
      local_shooter_cooling_heat_ -= shooter_cooling_rate_ * 0.1;
    if (local_shooter_cooling_heat_ < 0.0)
    {
      local_shooter_cooling_heat_ = 0.0;
      once_shoot_num_ = 0;
    }
    local_heat_pub_data_.once_shoot_num = once_shoot_num_;
    local_heat_pub_data_.local_shooter_cooling_heat = local_shooter_cooling_heat_;
    local_heat_pub_data_.desire_shoot_frequency = shoot_frequency_;
    local_heat_pub_.publish(local_heat_pub_data_);
  }
 
  void setStatusOfShooter(const rm_msgs::GameRobotStatus data)
  {
    shooter_cooling_limit_ = data.shooter_cooling_limit - heat_protect_threshold_;
    shooter_cooling_rate_ = data.shooter_cooling_rate;
  }
 
  void setCoolingHeatOfShooter(const rm_msgs::PowerHeatData data)
  {
    if (type_ == "ID1_17MM")
    {
      shooter_cooling_heat_ = data.shooter_id_1_17_mm_cooling_heat;
    }
    else if (type_ == "ID2_17MM")
    {
      // RoboMaster 2026 protocol only reports one 17mm barrel heat field.
      shooter_cooling_heat_ = data.shooter_id_1_17_mm_cooling_heat;
    }
    else if (type_ == "ID1_42MM")
    {
      shooter_cooling_heat_ = data.shooter_id_1_42_mm_cooling_heat;
    }
    referee_status_change_ = true;
  }
 
  void setRefereeStatus(bool status)
  {
    referee_is_online_ = status;
  }
 
  double getShootFrequency()
  {
    std::lock_guard<std::mutex> lock(heat_mutex_);
    if (!referee_is_online_)
      return 5.0;
    if (referee_status_change_ && abs(local_shooter_cooling_heat_ - shooter_cooling_heat_) > 10)
    {
      local_shooter_cooling_heat_ = shooter_cooling_heat_;
    }
    referee_status_change_ = false;
    double shooter_cooling_heat =
        (use_local_heat_ || !referee_is_online_) ? local_shooter_cooling_heat_ : shooter_cooling_heat_;
    if (shooter_cooling_limit_ - shooter_cooling_heat < bullet_heat_)
      return 0.0;
    else if (shooter_cooling_limit_ - shooter_cooling_heat == bullet_heat_)
      return shooter_cooling_rate_ / bullet_heat_;
    else if (shooter_cooling_limit_ - shooter_cooling_heat <= bullet_heat_ * heat_coeff_)
      return (shooter_cooling_limit_ - shooter_cooling_heat) / (bullet_heat_ * heat_coeff_) *
                 (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) +
             shooter_cooling_rate_ / bullet_heat_;
    else
      return shoot_frequency_;
  }
 
  int getSpeedLimit()
  {
    updateExpectShootFrequency();
    if (type_ == "ID1_17MM")
      return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND;
    else if (type_ == "ID2_17MM")
      return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND;
    else if (type_ == "ID1_42MM")
      return rm_msgs::ShootCmd::SPEED_16M_PER_SECOND;
    return -1;  // TODO unsafe!
  }
 
  int getCoolingLimit()
  {
    return shooter_cooling_limit_;
  }
 
  int getCoolingHeat()
  {
    return shooter_cooling_heat_;
  }
 
  void setShootFrequency(uint8_t mode)
  {
    state_ = mode;
  }
 
  int getShootFrequencyMode() const
  {
    return state_;
  }
 
  void flyTimeCB(const std_msgs::Float64ConstPtr& msg)
  {
    buff_shoot_frequency_ = 1 / (msg->data + buff_offset);
  }
 
private:
  void updateExpectShootFrequency()
  {
    if (state_ == HeatLimit::BURST)
    {
      shoot_frequency_ = burst_shoot_frequency_;
      burst_flag_ = true;
    }
    else if (state_ == HeatLimit::LOW)
    {
      shoot_frequency_ = low_shoot_frequency_;
      burst_flag_ = false;
    }
    else if (state_ == HeatLimit::HIGH)
    {
      shoot_frequency_ = high_shoot_frequency_;
      burst_flag_ = false;
    }
    else if (state_ == HeatLimit::MINIMAL)
    {
      shoot_frequency_ = buff_shoot_frequency_;
      // shoot_frequency_ = minimal_shoot_frequency_;
      burst_flag_ = false;
    }
    else
    {
      shoot_frequency_ = safe_shoot_frequency_;
      burst_flag_ = false;
    }
  }
 
  uint8_t state_{};
  std::string type_{};
  bool burst_flag_ = false;
  double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, low_shoot_frequency_{},
      high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}, outpost_shoot_frequency_{};
 
  bool referee_is_online_, use_local_heat_, last_shoot_state_{};
  int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_;
  double local_shooter_cooling_heat_{}, heat_protect_threshold_{};
  double buff_shoot_frequency_{}, buff_offset_{};
  int once_shoot_num_{};
  bool referee_status_change_{};
 
  ros::Publisher local_heat_pub_;
  ros::Subscriber shoot_state_sub_;
  ros::Subscriber fly_time_sub_;
  ros::Timer timer_;
 
  rm_msgs::LocalHeatData local_heat_pub_data_;
 
  mutable std::mutex heat_mutex_;
};
 
}  // namespace rm_common