void process_actionOfCups(Objects detectObject, Mat mat, TextBlock label_msg, TextBlock label_s, float[] pos, int drawY1, int drawY2)
{
if (wantTrans == true)
{
pos = camToWorkSpace(pos);
}
detectObject.setPos_m(pos[0], pos[1], pos[2]);
Objects.states s = detectObject.State();
if (startDemo == true)
{
if (s == Objects.states.stop)
{
CvInvoke.Line(mat, new Point(timeTick, drawY1), new Point(timeTick, drawY2), new MCvScalar(50, 50, 50));
}
else if (s == Objects.states.move)
{
CvInvoke.Line(mat, new Point(timeTick, drawY1), new Point(timeTick, drawY2), new MCvScalar(50, 150, 150));
if (handing == detectObject)//如果拿著的東西跟移動的東西一樣，代表繼續移動
{
}
else//如果拿著的東西跟移動的東西"不"一樣，代表要新增東西
{
if (cups.All(cup => cup != handing))//代表前面不是杯子 這樣才會有place //第一次也會進入(因為handing會是null)
{
}
else//代表前面有東西
{//所以這個時候handing是前一個拿起的東西(已經被放下的那個)
this.Dispatcher.Invoke((Action)(() =>
{
ActionList.Add(new ActionBaseList("Place", handing.Name, new SolidColorBrush(Colors.Black), new SolidColorBrush(handing.color)));
LV_actionBase.Items.Add(ActionList[ActionList.Count() - 1]);
if (handing.Distanse(dripTrayPos) < dripTrayD)//代表在drip tray上
{
ActionList.Add(new ActionBaseList(" to", subactInfo.place.DripTray.ToString(), new SolidColorBrush(Colors.Black), new SolidColorBrush(Colors.Black)));
LV_actionBase.Items.Add(ActionList[ActionList.Count() - 1]);
}
else
{
ActionList.Add(new ActionBaseList(" to", (handing.getNowPos()).ToString("mm", "3(", "0"), new SolidColorBrush(Colors.Black), new SolidColorBrush(handing.color)));
LV_actionBase.Items.Add(ActionList[ActionList.Count() - 1]);
}
}));
//ActionBaseAdd("Place", handing.Name, color1: Color.FromArgb(240, 230, 176), color2: handing.color);
}
this.Dispatcher.Invoke((Action)(() =>
{
if (detectObject.Distanse(dripTrayPos) < dripTrayD)//代表在drip tray上
{
ActionList.Add(new ActionBaseList("Pick up", subactInfo.place.DripTray.ToString(), new SolidColorBrush(Colors.Black), new SolidColorBrush(Colors.Black)));
LV_actionBase.Items.Add(ActionList[ActionList.Count() - 1]);
}
else
{
ActionList.Add(new ActionBaseList("Pick up", detectObject.Name, new SolidColorBrush(Colors.Black), new SolidColorBrush(detectObject.color)));
LV_actionBase.Items.Add(ActionList[ActionList.Count() - 1]);
}
}));
// ActionBaseAdd("Pick up", detectObject.Name, color1: Color.FromArgb(255, 200, 14), color2: detectObject.color);
}
handing = detectObject;
}
}//startDemo == true
this.Dispatcher.Invoke((Action)(() =>
{
label_s.Text = s.ToString();
label_msg.Text = ($"{ (detectObject.getX_m() * 1000).ToString("0.0")},{(detectObject.getY_m() * 1000).ToString("0.0")},{(detectObject.getZ_m() * 1000).ToString("0.0")}mm");
}));
}
private void MixToolStripMenuItem_Click(object sender, EventArgs e)
{
Pipeline pipe = new Pipeline();
var cfg = new Config();
cfg.EnableStream(Stream.Depth, 1280, 720, Format.Z16);
cfg.EnableStream(Stream.Color, 1280, 720, Format.Bgr8);
var pp = pipe.Start(cfg);
Task.Run(() =>
{
mat_color = new Mat(720, 1280, DepthType.Cv8U, 3);
mat_color_map = new Mat(mappingColor, DepthType.Cv8U, 3);
mat_depth = new Mat(720, 1280, DepthType.Cv8U, 1);
mat_show = new Mat(720, 1280, DepthType.Cv8U, 3);
yoloWrapper = new YoloWrapper("modle\\yolov3-tiny-3obj.cfg", "modle\\yolov3-tiny-3obj_3cup.weights", "modle\\obj.names");
string detectionSystemDetail = string.Empty;
if (!string.IsNullOrEmpty(yoloWrapper.EnvironmentReport.GraphicDeviceName))
{
detectionSystemDetail = $"({yoloWrapper.EnvironmentReport.GraphicDeviceName})";
}
Console.WriteLine($"Detection System:{yoloWrapper.DetectionSystem}{detectionSystemDetail}");
loop = true;
while (loop)
{
using (var frames = pipe.WaitForFrames())
{
mat_color = RS_color(frames.ColorFrame.DisposeWith(frames), matrix);
mat_depth = RS_depth(frames.DepthFrame);
Mat mat_depth_c3 = new Mat(720, 1280, DepthType.Cv8U, 3);
CvInvoke.CvtColor(mat_depth, mat_depth_c3, ColorConversion.Gray2Bgr);
CvInvoke.AddWeighted(mat_color, 0.5, mat_depth_c3, 0.5, 0.0, mat_show);
CvInvoke.Imwrite("yolo1.png", mat_color);
try { items = yoloWrapper.Detect(@"yolo1.png"); }
catch { break; }
foreach (YoloItem item in items)
{
string name = item.Type;
int x = item.X;
int y = item.Y;
int H = item.Height;
int W = item.Width;
Point center = item.Center();
CvInvoke.PutText(mat_show, name, new Point(x, y), FontFace.HersheySimplex, 1, new MCvScalar(50, 230, 230));
CvInvoke.PutText(mat_show, item.Confidence.ToString("0.0"), new Point(x, y - 20), FontFace.HersheySimplex, 0.5, new MCvScalar(50, 230, 230));
CvInvoke.Rectangle(mat_show, new Rectangle(x, y, W, H), new MCvScalar(50, 230, 230), 3);
byte depth = MyInvoke.GetValue <byte>(mat_depth, center.Y, center.X);
if (name == "blue cup")//index 0
{
process(cups[0], mat_Bcup, label_Bcup_msg, label_Bcup_state);
}
else if (name == "pink cup")//index 1
{
process(cups[1], mat_Pcup, label_Pcup_msg, label_Pcup_state);
}
void process(Objects detectObject, Mat mat, Label label_msg, Label label_s)
{
detectObject.setPos_mm(0, 0, (float)depth.Unit_mm());
Objects.states s = detectObject.State();
this.Invoke((MethodInvoker)(() => label_s.Text = s.ToString()));
if (s == Objects.states.stop)
{
CvInvoke.Line(mat, new Point(timeTick, 30), new Point(timeTick, 70), new MCvScalar(50, 50, 50));
}
else if (s == Objects.states.move)
{
CvInvoke.Line(mat, new Point(timeTick, 30), new Point(timeTick, 70), new MCvScalar(50, 150, 150));
if (handing == detectObject)//如果拿著的東西跟移動的東西一樣，代表繼續移動
{
}
else//如果拿著的東西跟移動的東西"不"一樣，代表要新增東西
{
//if (handing == new Objects())//代表第一次
if (cups.All(cup => cup != handing))//代表前面不是杯子 這樣才會有place
{
}
else//代表前面有東西
{
ActionBaseAdd("Place", handing.Name, color1: Color.FromArgb(240, 230, 176), color2: handing.color);
}
ActionBaseAdd("Pick up", detectObject.Name, color1: Color.FromArgb(255, 200, 14), color2: detectObject.color);
}
handing = detectObject;
}
this.Invoke((MethodInvoker)(() => label_msg.Text = (detectObject.getZ_m() * 1000).ToString("0.0") + "mm"));
}
//CvInvoke.PutText(mat_show, (depth.Unit_mm()).ToString("0.00") + "mm", center, FontFace.HersheySimplex, 1, new MCvScalar(50, 230, 230));
} //foreach cups
timeTick++;
} //using depth image
imageBox_Pcup.Image = mat_Pcup;
imageBox_Bcup.Image = mat_Bcup;
imageBox_RS.Image = mat_show;
}
});
}
