public void RequestSetParams(string rosDeviceName)
{
GameObject target = GameObject.Find(rosDeviceName);
Vector3 RosPosition = CoordinateConvert.UnityToRos(target.transform.position);
string pos_x_key = $"/lidars/{rosDeviceName}/transform/translation/x";
string pos_y_key = $"/lidars/{rosDeviceName}/transform/translation/y";
string pos_z_key = $"/lidars/{rosDeviceName}/transform/translation/z";
string pos_x_value = RosPosition.x.ToString();
string pos_y_value = RosPosition.y.ToString();
string pos_z_value = RosPosition.z.ToString();
// ��]��quaternion�ŕ\�������
Quaternion RosQuaternion = CoordinateConvert.UnityToRos(target.transform.rotation);
string rotation_x_key = $"/lidars/{rosDeviceName}/transform/rotation/x";
string rotation_y_key = $"/lidars/{rosDeviceName}/transform/rotation/y";
string rotation_z_key = $"/lidars/{rosDeviceName}/transform/rotation/z";
string rotation_w_key = $"/lidars/{rosDeviceName}/transform/rotation/w";
string rotation_x_value = RosQuaternion.x.ToString();
string rotation_y_value = RosQuaternion.y.ToString();
string rotation_z_value = RosQuaternion.z.ToString();
string rotation_w_value = RosQuaternion.w.ToString();
SetSingleValueToRosparam(pos_x_key, pos_x_value);
SetSingleValueToRosparam(pos_y_key, pos_y_value);
SetSingleValueToRosparam(pos_z_key, pos_z_value);
SetSingleValueToRosparam(rotation_x_key, rotation_x_value);
SetSingleValueToRosparam(rotation_y_key, rotation_y_value);
SetSingleValueToRosparam(rotation_z_key, rotation_z_value);
SetSingleValueToRosparam(rotation_w_key, rotation_w_value);
}
public void RequestGetParams(string rosDeviceName)
{
string translation_key = $"/lidars/{rosDeviceName}/transform/translation";
rosConnection.RosSocket.CallService <rosapi.GetParamRequest, rosapi.GetParamResponse>("/rosapi/get_param",
(rosapi.GetParamResponse res) => {
Debug.Log("GetParamResponse node: " + rosDeviceName);
Debug.Log("Value:" + res.value);
string[] values = res.value.Split(new char[] { '{', '}' });
if (values.Length != 3)
{
return;
}
string[] xyz = values[1].Split(',');
if (xyz.Length != 3)
{
return;
}
float x = float.Parse(xyz[0].Split(':')[1]);
float y = float.Parse(xyz[1].Split(':')[1]);
float z = float.Parse(xyz[2].Split(':')[1]);
Vector3 RosPosition = new Vector3(x, y, z);
Vector3 UnityPosition = CoordinateConvert.RosToUnity(RosPosition);
translationInViewerUpdatesRequests.Add(rosDeviceName, UnityPosition);
},
new rosapi.GetParamRequest(translation_key, "0"));
string rotation_key = $"/lidars/{rosDeviceName}/transform/rotation";
rosConnection.RosSocket.CallService <rosapi.GetParamRequest, rosapi.GetParamResponse>("/rosapi/get_param",
(rosapi.GetParamResponse res) => {
Debug.Log("GetParamResponse");
Debug.Log("GetParamResponse node: " + rosDeviceName);
string[] values = res.value.Split(new char[] { '{', '}' });
if (values.Length != 3)
{
return;
}
string[] xyzw = values[1].Split(',');
if (xyzw.Length != 4)
{
return;
}
float x = float.Parse(xyzw[0].Split(':')[1]);
float y = float.Parse(xyzw[1].Split(':')[1]);
float z = float.Parse(xyzw[2].Split(':')[1]);
float w = float.Parse(xyzw[3].Split(':')[1]);
Quaternion RosQuarternion = new Quaternion(x, y, z, w);
Quaternion UnityQuarternion = CoordinateConvert.RosToUnity(RosQuarternion);
rotationInviewerUpdatesRequests.Add(rosDeviceName, UnityQuarternion);
},
new rosapi.GetParamRequest(rotation_key, "0"));
}
protected override void ReceiveMessage(BoundingBoxes message)
{
Debug.Log($"Recieve BoundingBoxes: {message.bboxes.Length}");
Dictionary <int, Bounds> boundsDict = new Dictionary <int, Bounds>();
for (int i = 0; i < message.bboxes.Length; i++)
{
BoundingBox bbox = message.bboxes[i];
Vector3 minPtRos = new Vector3(bbox.xmin, bbox.ymin, bbox.zmin);
Vector3 maxnPtRos = new Vector3(bbox.xmax, bbox.ymax, bbox.zmax);
Vector3 minPt = CoordinateConvert.RosToUnity(minPtRos);
Vector3 maxPt = CoordinateConvert.RosToUnity(maxnPtRos);
Bounds bounds = new Bounds();
bounds.SetMinMax(minPt, maxPt);
boundsDict.Add((int)bbox.bbox_id, bounds);
}
NewBoundingBoxesListeners.Invoke(boundsDict);
}
void Update()
{
if (!Enabled)
{
regionsFromRosparam = null;
nowLoadingFromRosparam = false;
loadedFromRosparamAndWaitingProcessed = false;
loadedAndProcessed = false;
if (regionGameObjects != null && regionGameObjects.Length > 0)
{
foreach (var go in regionGameObjects)
{
Destroy(go);
}
regionGameObjects = null;
}
return;
}
// ====
// まずはROSPARM側の情報を読んでViewerに反映する
// ====
if (regionsFromRosparam == null && !nowLoadingFromRosparam)
{
// 読み込みリクエスト
GetRegionsFromRosparam();
nowLoadingFromRosparam = true;
return;
}
else if (regionsFromRosparam != null && nowLoadingFromRosparam)
{
// 読み込み完了・処理リクエスト
nowLoadingFromRosparam = false;
loadedFromRosparamAndWaitingProcessed = true;
return;
}
else if (loadedFromRosparamAndWaitingProcessed)
{
loadedFromRosparamAndWaitingProcessed = false;
regionGameObjects = new GameObject[regionsFromRosparam.Length];
Debug.Log($"ROSPARM LENGTH: {regionsFromRosparam.Length}");
for (int index = 0; index < regionsFromRosparam.Length; index++)
{
Debug.Log($"Region {index} convert to Unity GameObj");
Region r = regionsFromRosparam[index];
GameObject go = CreateGameObject.CubeRegion();
go.tag = "RegionForeground";
Vector3 maxPtRos = new Vector3(r.maxPt.x, r.maxPt.y, r.maxPt.z);
Vector3 minPtRos = new Vector3(r.minPt.x, r.minPt.y, r.minPt.z);
// 変換によって必ずしもmax, min pointでなくなる点に注意
Vector3 p1 = CoordinateConvert.RosToUnity(maxPtRos);
Vector3 p2 = CoordinateConvert.RosToUnity(minPtRos);
Vector3 pos = (p1 + p2) / 2;
Vector3 scale = new Vector3(Mathf.Abs(p1.x - p2.x), Mathf.Abs(p1.y - p2.y), Mathf.Abs(p1.z - p2.z));
go.transform.position = pos;
go.transform.localScale = scale;
regionGameObjects[index] = go;
}
loadedAndProcessed = true;
return;
}
// ====
// 現状のUnityサイドの情報を定期的にROSPARMに反映する
// ====
if (loadedAndProcessed)
{
if (!UpdateRegions)
{
return;
}
Debug.Log("Update Regions");
/*
* if (CheckCount > 0)
* {
* CheckCount -= 1;
* return;
* }
*/
UpdateRegions = false;
CheckCount = UpdateInterval;
SetRegionsToRosparam();
}
}
private void SetRegionsToRosparam()
{
Debug.Log("SetRegionsToRosparam");
regionGameObjects = GameObject.FindGameObjectsWithTag("RegionForeground");
Region[] regions = new Region[regionGameObjects.Length];
bool flag_changed = false;
if (regionGameObjects.Length != regionsFromRosparam.Length)
{
flag_changed = true;
}
for (int i = 0; i < regionGameObjects.Length; i++)
{
Vector3 p = CoordinateConvert.UnityToRos(regionGameObjects[i].transform.position);
Vector3 _s = CoordinateConvert.UnityToRos(regionGameObjects[i].transform.localScale);
Vector3 s = new Vector3(Mathf.Abs(_s.x), Mathf.Abs(_s.y), Mathf.Abs(_s.z));
Quaternion q = CoordinateConvert.UnityToRos(regionGameObjects[i].transform.rotation);
Vector3 minPt = new Vector3(p.x - s.x / 2, p.y - s.y / 2, p.z - s.z / 2);
Vector3 maxPt = new Vector3(p.x + s.x / 2, p.y + s.y / 2, p.z + s.z / 2);
regions[i] = new Region();
regions[i].minPt = new Point(minPt.x, minPt.y, minPt.z);
regions[i].maxPt = new Point(maxPt.x, maxPt.y, maxPt.z);
regions[i].rotation = new Point(q.eulerAngles.x, q.eulerAngles.y, q.eulerAngles.z);
if (!flag_changed && JsonUtility.ToJson(regions[i]) != JsonUtility.ToJson(regionsFromRosparam[i]))
{
flag_changed = true;
}
}
if (!flag_changed)
{
return;
}
if (rosConnection != null && rosConnection.RosSocket != null)
{
for (int i = 0; i < regions.Length; i++)
{
string key_min_x = $"/regions/foregrounds/{i}/minPt/x";
string key_min_y = $"/regions/foregrounds/{i}/minPt/y";
string key_min_z = $"/regions/foregrounds/{i}/minPt/z";
string key_max_x = $"/regions/foregrounds/{i}/maxPt/x";
string key_max_y = $"/regions/foregrounds/{i}/maxPt/y";
string key_max_z = $"/regions/foregrounds/{i}/maxPt/z";
string key_rotation_x = $"/regions/foregrounds/{i}/rotation/x";
string key_rotation_y = $"/regions/foregrounds/{i}/rotation/y";
string key_rotation_z = $"/regions/foregrounds/{i}/rotation/z";
string value_min_x = regions[i].minPt.x.ToString("F4");
string value_min_y = regions[i].minPt.y.ToString("F4");
string value_min_z = regions[i].minPt.z.ToString("F4");
string value_max_y = regions[i].maxPt.y.ToString("F4");
string value_max_x = regions[i].maxPt.x.ToString("F4");
string value_max_z = regions[i].maxPt.z.ToString("F4");
string value_rot_y = regions[i].rotation.x.ToString("F4");
string value_rot_x = regions[i].rotation.y.ToString("F4");
string value_rot_z = regions[i].rotation.z.ToString("F4");
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_min_x, value_min_x));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_min_y, value_min_y));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_min_z, value_min_z));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_max_x, value_max_x));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_max_y, value_max_y));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_max_z, value_max_z));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_rotation_x, value_rot_x));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_rotation_y, value_rot_y));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_rotation_z, value_rot_z));
Debug.Log("Set: " + JsonUtility.ToJson(regions[i]));
}
// rosparam側に多い分は削除する
for (int i = regions.Length; i < regionsFromRosparam.Length; i++)
{
string delete_key = $"/regions/foregrounds/{i}";
rosConnection.RosSocket.CallService <rosapi.DeleteParamRequest, rosapi.DeleteParamResponse>("/rosapi/delete_param", deleteParamResultHandler, new rosapi.DeleteParamRequest(delete_key));
Debug.Log("delete key");
}
regionsFromRosparam = regions;
}
}
protected override void ReceiveMessage(PointCloud2 message)
{
uint x_offset = 0;
uint y_offset = 4;
uint z_offset = 8;
uint color_offset = 16;
bool colorEnabled = false;
for (int j = 0; j < message.fields.Length; j++)
{
PointField field = message.fields[j];
if (field.name == "x")
{
x_offset = field.offset;
}
else if (field.name == "y")
{
y_offset = field.offset;
}
else if (field.name == "z")
{
z_offset = field.offset;
}
else if (field.name == "rgb")
{
color_offset = field.offset;
colorEnabled = true;
}
}
size = message.data.GetLength(0);
int i = 0;
byteArray = new byte[size];
foreach (byte temp in message.data)
{
byteArray[i] = temp; //byte型を取得
i++;
}
point_step = message.point_step;
size = size / (int)point_step;
// PointCloud2生データをUnity座標系の3次元点群に変換
Vector3[] pcl = new Vector3[size];
Color[] colors = new Color[size];
for (int n = 0; n < size; n++)
{
// yte型をfloatに変換
int x_posi = n * (int)point_step + (int)x_offset;
int y_posi = n * (int)point_step + (int)y_offset;
int z_posi = n * (int)point_step + (int)z_offset;
int color_posi = n * (int)point_step + (int)color_offset;
float x = BitConverter.ToSingle(byteArray, x_posi);
float y = BitConverter.ToSingle(byteArray, y_posi);
float z = BitConverter.ToSingle(byteArray, z_posi);
Vector3 RosPosition = new Vector3(x, y, z);
Vector3 UnityPosition = CoordinateConvert.RosToUnity(RosPosition);
pcl[n] = UnityPosition;
//
if (colorEnabled)
{
byte b = byteArray[color_posi];
byte g = byteArray[color_posi + 1];
byte r = byteArray[color_posi + 2];
colors[n] = new Color(r / 255f, g / 255f, b / 255f);
}
}
this.pointCloud = pcl;
this.rgbColors = colorEnabled ? colors : null;
NewPointCloudListeners.Invoke(pointCloud, rgbColors);
}