121 lines
4.2 KiB
C#
121 lines
4.2 KiB
C#
using DG.Tweening;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
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;
|
||
// 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 (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();
|
||
//Debug.Log(Newtonsoft.Json.JsonConvert.SerializeObject(current_robot_pos));
|
||
|
||
/*display_x = (current_robot_pos.current_pose.x - origin_x) / resolution;
|
||
display_y = (current_robot_pos.current_pose.x - origin_y) / resolution;
|
||
|
||
display_x *= (rect_transform.sizeDelta.x / pixel_width);
|
||
display_y *= (rect_transform.sizeDelta.y / pixel_height);*/
|
||
|
||
|
||
// display_x = - bmp_x + map_x/resolution
|
||
display_x = -origin_x + current_robot_pos.current_pose.x / resolution;
|
||
// display _y= -（bmp_h - bmp_y + map_y/resolution）
|
||
display_y = -(pixel_height - origin_y + current_robot_pos.current_pose.y / resolution);
|
||
|
||
var _display_y = (display_x / pixel_width) * (296) + 50;
|
||
//var _display_x = MathF.Abs(((float)display_y / (float)pixel_height) * 750) - 790;
|
||
var _display_x = MathF.Abs(790 - MathF.Abs(((float)display_y / (float)pixel_height) * 785));
|
||
|
||
//rect_transform.DOAnchorPos(new Vector2((float)display_x, (float)display_y), 0.5f).OnComplete(() =>
|
||
rect_transform.DOAnchorPos(new Vector2((float)_display_x + 24, (float)_display_y + 24), 0.5f).OnComplete(() =>
|
||
{
|
||
//Debug.Log($"robot位置轨迹：{new Vector2((float)display_x, (float)display_y)}");
|
||
Debug.Log($"robot位置轨迹：{new Vector2((float)_display_x + 24, (float)_display_y + 24)}");
|
||
RobotPosQueue.Dequeue();
|
||
current_robot_pos = null;
|
||
});
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
current_robot_pos = null;
|
||
Debug.Log($"robot位置轨迹错误：{e.Message}，行次：{e.StackTrace}");
|
||
}
|
||
|
||
}
|
||
}
|
||
}
|