static enum hrtimer_restart tx_timer_func(struct hrtimer *timer)
{
	struct mem_link_device *mld;
	struct link_device *ld;
	struct modem_ctl *mc;
	int i;
	bool need_schedule;
	u16 mask;

	mld = container_of(timer, struct mem_link_device, tx_timer);
	ld = &mld->link_dev;
	mc = ld->mc;

	if (unlikely(cp_online(mc) && !ipc_active(mld)))
		goto exit;

	need_schedule = false;
	mask = 0;

	for (i = IPC_FMT; i < MAX_SIPC5_DEV; i++) {
		struct mem_ipc_device *dev = mld->dev[i];
		int ret;

		if (unlikely(under_tx_flow_ctrl(mld, dev))) {
			ret = check_tx_flow_ctrl(mld, dev);
			if (ret < 0) {
				if (ret == -EBUSY || ret == -ETIME) {
					need_schedule = true;
					continue;
				} else {
					mem_forced_cp_crash(mld);
					goto exit;
				}
			}
		}

		ret = tx_frames_to_dev(mld, dev);
		if (unlikely(ret < 0)) {
			if (ret == -EBUSY || ret == -ENOSPC) {
				need_schedule = true;
				start_tx_flow_ctrl(mld, dev);
				continue;
			} else {
				mem_forced_cp_crash(mld);
				goto exit;
			}
		}

		mask |= msg_mask(dev);

		if (!skb_queue_empty(dev->skb_txq))
			need_schedule = true;
	}

	if (mask)
		send_ipc_irq(mld, mask2int(mask));

	if (need_schedule) {
		ktime_t ktime = ns_to_ktime(ms2ns(TX_PERIOD_MS));
		hrtimer_forward_now(timer, ktime);
		return HRTIMER_RESTART;
	}

exit:
#ifdef CONFIG_LINK_POWER_MANAGEMENT
	mld->permit_cp_sleep(mld);
#endif
	return HRTIMER_NORESTART;
}
static enum hrtimer_restart tx_timer_func(struct hrtimer *timer)
{
	struct mem_link_device *mld;
	struct link_device *ld;
	struct modem_ctl *mc;
	int i;
	bool need_schedule;
	u16 mask;
	unsigned long flags;

	mld = container_of(timer, struct mem_link_device, tx_timer);
	ld = &mld->link_dev;
	mc = ld->mc;

	need_schedule = false;
	mask = 0;

	spin_lock_irqsave(&mc->lock, flags);

	if (unlikely(!ipc_active(mld)))
		goto exit;

#ifdef CONFIG_LINK_POWER_MANAGEMENT_WITH_FSM
	if (mld->link_active) {
		if (!mld->link_active(mld)) {
			need_schedule = true;
			goto exit;
		}
	}
#endif

	for (i = 0; i < MAX_SIPC5_DEVICES; i++) {
		struct mem_ipc_device *dev = mld->dev[i];
		int ret;

		if (unlikely(under_tx_flow_ctrl(mld, dev))) {
			ret = check_tx_flow_ctrl(mld, dev);
			if (ret < 0) {
				if (ret == -EBUSY || ret == -ETIME) {
					need_schedule = true;
					continue;
				} else {
					mem_forced_cp_crash(mld);
					need_schedule = false;
					goto exit;
				}
			}
		}

		ret = tx_frames_to_dev(mld, dev);
		if (unlikely(ret < 0)) {
			if (ret == -EBUSY || ret == -ENOSPC) {
				need_schedule = true;
				start_tx_flow_ctrl(mld, dev);
				continue;
			} else {
				mem_forced_cp_crash(mld);
				need_schedule = false;
				goto exit;
			}
		}

		if (ret > 0)
			mask |= msg_mask(dev);

		if (!skb_queue_empty(dev->skb_txq))
			need_schedule = true;
	}

	if (!need_schedule) {
		for (i = 0; i < MAX_SIPC5_DEVICES; i++) {
			if (!txq_empty(mld->dev[i])) {
				need_schedule = true;
				break;
			}
		}
	}

	if (mask)
		send_ipc_irq(mld, mask2int(mask));

exit:
	if (need_schedule) {
		ktime_t ktime = ktime_set(0, ms2ns(TX_PERIOD_MS));
		hrtimer_start(timer, ktime, HRTIMER_MODE_REL);
	}

	spin_unlock_irqrestore(&mc->lock, flags);

	return HRTIMER_NORESTART;
}
  void patchArrayCallback(const samplereturn_msgs::PatchArrayConstPtr& msg)
  {
    samplereturn_msgs::NamedPointArray points_out;
    points_out.header = msg->header;
    bool enable_debug = (pub_debug_image.getNumSubscribers() != 0);
    cv::Mat debug_image;
    if (enable_debug) {
      debug_image = cv::Mat::zeros(msg->cam_info.height,
          msg->cam_info.width, CV_8UC3);
    }
    for (size_t i = 0; i < msg->patch_array.size(); i++) {
      cv_bridge::CvImageConstPtr cv_ptr_mask, cv_ptr_img;
      sensor_msgs::ImageConstPtr msg_mask(&(msg->patch_array[i].mask), boost::serialization::null_deleter());
      sensor_msgs::ImageConstPtr msg_img(&(msg->patch_array[i].image), boost::serialization::null_deleter());
      try {
        cv_ptr_mask = cv_bridge::toCvShare(msg_mask, "mono8");
      }
      catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge mask exception: %s", e.what());
      }
      try {
        cv_ptr_img = cv_bridge::toCvShare(msg_img, "rgb8");
      }
      catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge image exception: %s", e.what());
      }
      if (enable_debug) {
        cv_ptr_img->image.copyTo(debug_image(cv::Rect(msg->patch_array[i].image_roi.x_offset,
                msg->patch_array[i].image_roi.y_offset,
                msg->patch_array[i].image_roi.width,
                msg->patch_array[i].image_roi.height)));
      }

      // check foreground/background distance
      samplereturn::ColorModel cm(cv_ptr_img->image, cv_ptr_mask->image);
      samplereturn::HueHistogram hh_inner = cm.getInnerHueHistogram(config_.min_color_saturation, config_.low_saturation_limit, config_.high_saturation_limit);
      samplereturn::HueHistogram hh_outer = cm.getOuterHueHistogram(config_.min_color_saturation, config_.low_saturation_limit, config_.high_saturation_limit);
      double background_distance = hh_inner.distance(hh_outer);
    
      // compare background to fence color
      std::vector<std::tuple<double, double>> edges;
      edges.push_back(std::make_tuple(0, config_.max_fence_hue));
      samplereturn::HueHistogram hh_fence_measured = cm.getOuterHueHistogram(config_.min_fence_color_saturation,0.0, config_.fence_high_saturation_limit);
      samplereturn::HueHistogramExemplar hh_fence_exemplar = samplereturn::ColorModel::getColoredSampleModel(edges, 0.0, config_.fence_high_saturation_limit);
      double fence_distance = hh_fence_exemplar.distance(hh_fence_measured);

      // Check inner hist against targets, either well-saturated in the specified
      // hue range, or unsaturated with a high value (metal and pre-cached)
      edges.clear();
      edges.push_back(std::make_tuple(0, config_.max_target_hue));
      edges.push_back(std::make_tuple(config_.min_target_hue, 180));
      samplereturn::HueHistogramExemplar hh_colored_sample = samplereturn::ColorModel::getColoredSampleModel(edges, config_.low_saturation_limit, config_.high_saturation_limit);
      samplereturn::HueHistogramExemplar hh_value_sample = samplereturn::ColorModel::getValuedSampleModel(config_.low_saturation_limit, config_.high_saturation_limit);
      double hue_exemplar_distance = hh_colored_sample.distance(hh_inner);
      double value_exemplar_distance = hh_value_sample.distance(hh_inner);

      if (enable_debug) {
          int x,y,h;
          x = msg->patch_array[i].image_roi.x_offset;
          y = msg->patch_array[i].image_roi.y_offset;
          //w = msg->patch_array[i].image_roi.width;
          h = msg->patch_array[i].image_roi.height;
          hh_inner.draw_histogram(debug_image, x, y, config_.debug_font_scale);
          hh_outer.draw_histogram(debug_image, x, y + h + 25*config_.debug_font_scale, config_.debug_font_scale);
          char edist[100];
          snprintf(edist, 100, "b:%3.2f h: %3.2f v:%3.2f f:%3.2f", background_distance, hue_exemplar_distance, value_exemplar_distance, fence_distance);
          cv::putText(debug_image,edist,cv::Point2d(x+70, y + h + 50*config_.debug_font_scale),
                  cv::FONT_HERSHEY_SIMPLEX, config_.debug_font_scale, cv::Scalar(255,0,0),4,cv::LINE_8);
      }

      // is a sample if high enough bg/fg distance, high enough fence distance,
      // and close to one of the exemplars
      bool is_sample = ((background_distance>config_.min_inner_outer_distance) &&
                        (fence_distance>config_.min_fence_distance) &&
                        ((hue_exemplar_distance<config_.max_exemplar_distance) ||
                         (value_exemplar_distance<config_.max_exemplar_distance)));

      if (!is_sample)
      {
          if(enable_debug)
          {
              // Nix region
              int x,y,w,h;
              x = msg->patch_array[i].image_roi.x_offset;
              y = msg->patch_array[i].image_roi.y_offset;
              w = msg->patch_array[i].image_roi.width;
              h = msg->patch_array[i].image_roi.height;
              cv::line(debug_image,
                      cv::Point2f(x, y),
                      cv::Point2f(x + w, y + h), cv::Scalar(255,0,0), 20);
              cv::line(debug_image,
                      cv::Point2f(x + w, y),
                      cv::Point2f(x, y + h), cv::Scalar(255,0,0), 20);
          }
          continue;
      }

      samplereturn_msgs::NamedPoint np_msg;
      np_msg.header.stamp = msg->header.stamp;
      np_msg.header.frame_id = msg->patch_array[i].world_point.header.frame_id;
      np_msg.point = msg->patch_array[i].world_point.point;
      hh_inner.to_msg(&np_msg.model.hue);
      np_msg.sensor_frame = msg->header.frame_id;
      np_msg.name = (hue_exemplar_distance < value_exemplar_distance) ?
          std::string("Hue object") : std::string("Value object");

      if(is_sample && config_.compute_grip_angle)
      {
          cv::RotatedRect griprect;
          if(samplereturn::computeGripAngle(cv_ptr_mask->image, &griprect, &np_msg.grip_angle) &&
                  enable_debug)
          {
              griprect.center += cv::Point2f(
                      msg->patch_array[i].image_roi.x_offset,
                      msg->patch_array[i].image_roi.y_offset);
              samplereturn::drawGripRect(debug_image, griprect);
          }
      }

      points_out.points.push_back(np_msg);
      
    }
    pub_named_points.publish(points_out);
    if (enable_debug) {
      sensor_msgs::ImagePtr debug_image_msg =
        cv_bridge::CvImage(msg->header,"rgb8",debug_image).toImageMsg();
      pub_debug_image.publish(debug_image_msg);
    }
  }
  void patchArrayCallback(const samplereturn_msgs::PatchArrayConstPtr& msg)
  {
    samplereturn_msgs::NamedPointArray points_out;
    points_out.header = msg->header;
    bool enable_debug = (pub_debug_image.getNumSubscribers() != 0);
    cv::Mat debug_image;
    if (enable_debug) {
      debug_image = cv::Mat::zeros(msg->cam_info.height,
          msg->cam_info.width, CV_8UC3);
    }
    for (size_t i = 0; i < msg->patch_array.size(); i++) {
      cv_bridge::CvImageConstPtr cv_ptr_mask, cv_ptr_img;
      sensor_msgs::ImageConstPtr msg_mask(&(msg->patch_array[i].mask), boost::serialization::null_deleter());
      sensor_msgs::ImageConstPtr msg_img(&(msg->patch_array[i].image), boost::serialization::null_deleter());
      try {
        cv_ptr_mask = cv_bridge::toCvShare(msg_mask, "mono8");
      }
      catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge mask exception: %s", e.what());
      }
      try {
        cv_ptr_img = cv_bridge::toCvShare(msg_img, "rgb8");
      }
      catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge image exception: %s", e.what());
      }
      if (enable_debug) {
        cv_ptr_img->image.copyTo(debug_image(cv::Rect(msg->patch_array[i].image_roi.x_offset,
                msg->patch_array[i].image_roi.y_offset,
                msg->patch_array[i].image_roi.width,
                msg->patch_array[i].image_roi.height)));
      }

      cv::Mat Lab;
      cv::cvtColor(cv_ptr_img->image, Lab, cv::COLOR_RGB2Lab);
      cv::Mat point(1,1,CV_32FC3);
      point = cv::mean(Lab, cv_ptr_mask->image);
      cv::Mat scaled_point = (point.reshape(1) - mean_)/std_;
      cv::Mat prediction;
      svm->predict(scaled_point, prediction, cv::ml::StatModel::RAW_OUTPUT);
      float distance = prediction.at<float>(0);

      if (enable_debug) {
          int x,y,h;
          x = msg->patch_array[i].image_roi.x_offset;
          y = msg->patch_array[i].image_roi.y_offset;
          //w = msg->patch_array[i].image_roi.width;
          h = msg->patch_array[i].image_roi.height;
          char edist[100];
          snprintf(edist, 100, "d:%3.2f", distance);
          cv::putText(debug_image,edist,cv::Point2d(x+70, y + h + 50*config_.debug_font_scale),
                  cv::FONT_HERSHEY_SIMPLEX, config_.debug_font_scale, cv::Scalar(255,0,0),4,cv::LINE_8);
      }

      bool is_sample = distance<0;

      if(!is_sample)
      {
          if(enable_debug)
          {
              // Nix region
              int x,y,w,h;
              x = msg->patch_array[i].image_roi.x_offset;
              y = msg->patch_array[i].image_roi.y_offset;
              w = msg->patch_array[i].image_roi.width;
              h = msg->patch_array[i].image_roi.height;
              cv::line(debug_image,
                      cv::Point2f(x, y),
                      cv::Point2f(x + w, y + h), cv::Scalar(255,0,0), 20);
              cv::line(debug_image,
                      cv::Point2f(x + w, y),
                      cv::Point2f(x, y + h), cv::Scalar(255,0,0), 20);
          }
          continue;
      }

      samplereturn_msgs::NamedPoint np_msg;

      if(config_.compute_grip_angle)
      {
          cv::RotatedRect griprect;
          if(samplereturn::computeGripAngle(cv_ptr_mask->image, &griprect, &np_msg.grip_angle) &&
                  enable_debug)
          {
              griprect.center += cv::Point2f(
                      msg->patch_array[i].image_roi.x_offset,
                      msg->patch_array[i].image_roi.y_offset);
              samplereturn::drawGripRect(debug_image, griprect);
          }
      }

      np_msg.header.stamp = msg->header.stamp;
      np_msg.header.frame_id = msg->patch_array[i].world_point.header.frame_id;
      np_msg.point = msg->patch_array[i].world_point.point;
      np_msg.sensor_frame = msg->header.frame_id;
      np_msg.name = "SVM";
      points_out.points.push_back(np_msg);
    }
    pub_named_points.publish(points_out);
    if (enable_debug) {
      sensor_msgs::ImagePtr debug_image_msg =
        cv_bridge::CvImage(msg->header,"rgb8",debug_image).toImageMsg();
      pub_debug_image.publish(debug_image_msg);
    }
  }