133 lines
4.3 KiB
C#
133 lines
4.3 KiB
C#
using DG.Tweening;
|
||
using Newtonsoft.Json;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.IO;
|
||
using UnityEngine;
|
||
|
||
/// <summary>
|
||
/// 机器人对象
|
||
/// </summary>
|
||
public class RobotObject : MonoBehaviour
|
||
{
|
||
public RectTransform rect_transform;
|
||
/*
|
||
bmp_x = origin.x
|
||
*/
|
||
/// <summary>
|
||
/// 机器人信息
|
||
/// </summary>
|
||
public RobotClass robotClass = null;
|
||
/// <summary>
|
||
/// 机器人地图数据
|
||
/// </summary>
|
||
public RobotMap RobotMap = null;
|
||
/// <summary>
|
||
/// 机器人位置状态信息
|
||
/// </summary>
|
||
public Queue<RobotPos> RobotPosQueue = new Queue<RobotPos>();
|
||
/// <summary>
|
||
/// 机器人任务队列
|
||
/// </summary>
|
||
public RobotTask RobotTask = null;
|
||
|
||
/// <summary>
|
||
/// 当前执行的机器人位置信息
|
||
/// 【map_x，map_y】
|
||
/// </summary>
|
||
private RobotPos current_robot_pos = null;
|
||
|
||
//固定值
|
||
public int origin_x;//=info.origin.x
|
||
public int origin_y;//=info.origin.y
|
||
public int pixel_width;//info.width
|
||
public int pixel_height;//info.height
|
||
public double resolution;//=info.resolution
|
||
//public double bmp_x;//=origin.x/resolution
|
||
//public double bmp_y;//=info.height-(origin.y/resolution)
|
||
//public float bmp_h;//=info.height
|
||
//public float bmp_w;//=info.width
|
||
|
||
//实时值
|
||
//public double map_x;//=current_pos.x
|
||
//public double map_y;//=current_pos.y
|
||
public double display_x;//=(current_pos.x-origin.x)/resolution
|
||
public double display_y;//=(current_pos.y-origin.y)/resolution
|
||
|
||
private bool get;
|
||
|
||
//public RobotMapInfoTwo robotMapInfo;
|
||
// Start is called before the first frame update
|
||
void Start()
|
||
{
|
||
rect_transform = GetComponent<RectTransform>();
|
||
//robotClass = null;
|
||
//RobotMap = null;
|
||
}
|
||
|
||
// Update is called once per frame
|
||
void Update()
|
||
{
|
||
#if UNITY_EDITOR
|
||
if (Input.GetKeyDown(KeyCode.A))
|
||
{
|
||
for (int i = 0; i < WebInteraction.Inst.robotPos.Count; i++)
|
||
{
|
||
RobotPosQueue.Enqueue(WebInteraction.Inst.robotPos[i]);
|
||
}
|
||
}
|
||
#endif
|
||
if (robotClass == null || RobotMap == null)
|
||
return;
|
||
if (!get && RobotMap.info != null)
|
||
{
|
||
get = true;
|
||
|
||
origin_x = RobotMap.info.origin.x;
|
||
origin_y = RobotMap.info.origin.y;
|
||
pixel_width = RobotMap.info.width;
|
||
pixel_height = RobotMap.info.height;
|
||
resolution = RobotMap.info.resolution;
|
||
|
||
Debug.Log($"origin_x：{origin_x}，origin_y：{origin_y}，pixel_width：{pixel_width}，pixel_height：{pixel_height}，resolution：{resolution}");
|
||
|
||
}
|
||
|
||
if (RobotPosQueue.Count > 0 && current_robot_pos == null)
|
||
{
|
||
try
|
||
{
|
||
current_robot_pos = RobotPosQueue.Peek();
|
||
string newData = JsonConvert.SerializeObject(current_robot_pos);
|
||
|
||
//Debug.Log("resolution:" + resolution);
|
||
//Debug.Log("current_robot_pos.current_pose.x:" + current_robot_pos.current_pose.x);
|
||
//Debug.Log("current_robot_pos.current_pose.y:" + current_robot_pos.current_pose.y);
|
||
//Debug.Log("origin_x:" + origin_x);
|
||
//Debug.Log("origin_y:" + origin_y);
|
||
//Debug.Log("x:" + (float)((current_robot_pos.current_pose.x - origin_x) / resolution));
|
||
//Debug.Log("y:" + (float)((current_robot_pos.current_pose.y - origin_y) / resolution));
|
||
|
||
display_x = MathF.Abs((float)((current_robot_pos.current_pose.x - origin_x) / resolution));
|
||
display_y = MathF.Abs((float)((current_robot_pos.current_pose.y - origin_y) / resolution));
|
||
|
||
rect_transform.DOAnchorPos(new Vector2(-(float)display_y, (float)display_x), 0.5f).OnComplete(() =>
|
||
{
|
||
Debug.Log($"robot位置轨迹：{-new Vector2((float)display_x, (float)display_y)}");
|
||
RobotPosQueue.Dequeue();
|
||
current_robot_pos = null;
|
||
});
|
||
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
current_robot_pos = null;
|
||
Debug.Log($"robot位置轨迹错误：{e.Message}，行次：{e.StackTrace}");
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
}
|