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);
}
}
