private void findNode()
{
double min = 999;
double robotX = Data.Instance.Robot_work_info["R_002"].robot_status_info.robotstate.msg.pose.x;
double robotY = Data.Instance.Robot_work_info["R_002"].robot_status_info.robotstate.msg.pose.y;
WorkFlowGoal temp_action = new WorkFlowGoal();
string missionname = "";
foreach (KeyValuePair <string, Node_mission> mission in Data.Instance.node_mission_list)
{
temp_action = JsonConvert.DeserializeObject <WorkFlowGoal>(mission.Value.work);
double mintemp = 0;
double nodeX = temp_action.work[0].action_args[0];
double nodeY = temp_action.work[0].action_args[1];
mintemp = onPointToPointDist(robotX, robotY, nodeX, nodeY);
string strTemp = Data.Instance.node_mission_list[mission.Key].mission_name;
if (min > mintemp)
{
min = mintemp;
startNode = strTemp;
}
}
Console.WriteLine("가장 가까운 노드는 : {0}, 거리 :{1}", missionname, min);
}
private void simpleButton3_Click(object sender, EventArgs e)
{
try
{
missioneditDlg missionedit = new missioneditDlg();
//mainform.dbBridge.
if (missionedit.ShowDialog() == DialogResult.OK)
{
string missionname = missionedit.strMissionName;
string missionid = missionedit.strMissionID;
string missionatc = missionedit.strMissionATC;
WorkFlowGoal missiondata = new WorkFlowGoal();
missiondata.work_id = missionid;
missiondata.action_start_idx = 0;
missiondata.loop_flag = 1;
Action act = new Action();
act.action_type = (int)Data.ACTION_TYPE.Goal_Point;
act.action_args.Add(0);
act.action_args.Add(0);
act.action_args.Add(0);
ParameterSet paramset = new ParameterSet();
paramset.param_name = "max_trans_vel";
paramset.type = "float";
paramset.value = "0.7";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "xy_goal_tolerance";
paramset.type = "float";
paramset.value = "0.2";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "yaw_goal_tolerance";
paramset.type = "float";
paramset.value = "0.05";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "p_drive";
paramset.type = "float";
paramset.value = "0.7";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "d_drive";
paramset.type = "float";
paramset.value = "1.2";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "wp_tolerance";
paramset.type = "float";
paramset.value = "1";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "avoid";
paramset.type = "bool";
paramset.value = "false";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "ATC_NUMBER";
paramset.type = "string";
paramset.value = "000";
act.action_params.Add(paramset);
missiondata.work.Add(act);
string strMissionData_Json = JsonConvert.SerializeObject(missiondata);
//미션리스트 DB저장
mainform.dbBridge.onDBInsert_Missionlist(missionname, missionid, "", "", strMissionData_Json);
//화면 갱신
}
}
catch
{
}
}
private void test()
{
try
{
int cnt = missionbuf_in.Count();
WorkFlowGoal workflowgoal = new WorkFlowGoal();
workflowgoal.work_id = "TEMP_";
workflowgoal.action_start_idx = 0;
workflowgoal.loop_flag = 1;
for (int i = 0; i < cnt; i++)
{
DB_MissionData db_missiondata = new DB_MissionData();
MisssionInfo missioninfo = new MisssionInfo();
string strmissionid = missionbuf_in[i];
missioninfo = mainform.dbBridge.onDBRead_Mission(strmissionid);
string strwork = missioninfo.work;
db_missiondata = JsonConvert.DeserializeObject <DB_MissionData>(strwork);
float x = db_missiondata.work[0].action_args[0];
float y = db_missiondata.work[0].action_args[1];
float theta = db_missiondata.work[0].action_args[2];
Action act = new Action();
act.action_type = (int)Data.ACTION_TYPE.Goal_Point;
act.action_args.Add(x);
act.action_args.Add(y);
act.action_args.Add(theta);
ParameterSet paramset = new ParameterSet();
paramset.param_name = "max_trans_vel";
paramset.type = "float";
paramset.value = "1.0";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "xy_goal_tolerance";
paramset.type = "float";
paramset.value = "0.15";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "yaw_goal_tolerance";
paramset.type = "float";
paramset.value = "0.05";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "p_drive";
paramset.type = "float";
paramset.value = "0.4";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "d_drive";
paramset.type = "float";
paramset.value = "1.2";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "wp_tolerance";
paramset.type = "float";
paramset.value = "1";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "avoid";
paramset.type = "bool";
paramset.value = "true";
act.action_params.Add(paramset);
paramset = new ParameterSet();
paramset.param_name = "passing_flag";
paramset.type = "bool";
paramset.value = "true";
act.action_params.Add(paramset);
workflowgoal.work.Add(act);
}
string strMissionData_Json = JsonConvert.SerializeObject(workflowgoal);
mainform.dbBridge.onDBInsert_Missionlist(DateTime.Now.ToString("LS_" + "yyyyMMddhhmm"), "TEMP_", "0", "", strMissionData_Json);
}
catch (Exception e)
{
Console.WriteLine("insert mission buf error -> {0}", e);
}
}
